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Abstract 


MADS (Minimization Assistant for Dynamical Systems) is a trajectory optimization code 
in which a user-specified performance measure is directly minimized, subject to constraints 
placed on a low-order discretization of user-supplied plant ordinary differential equations. 
This document describes the mathematical formulation of the set of trajectory optimization 
problems for which MADS is suitable, and describes the user interface. Usage examples are 
provided. 


1 



Contents 

11 Introduction] 

12 Problem Formulation and Software Interfacel 


2.1 

Excursus: Custom Problem Formulations in MADS| 

2.2 

Subroutines To Be Provided By The User 

2.3 

Producing and Operating On A MADS Solution| 


12.3.1 Formats for MADS Solution Datal 


2.3.2 Matlab Functions for Operating On MADS Data| 


2A Setting Up and Executing a MADS Run 


12.4.1 Autodifferentiation for MADSI 


|3 Tutorial Examples 

3.1 Linear System Minimum Time to Originl 


3.1.1 Baseline Problem! 

3.1.2 Break the problem into two phases 

3.1.3 Introduce variable discretization step size] 

3.1.4 Eliminate the Bangs 

3.1.5 Problem Summary 

IT2 

Goddard Problem| 


3.2.1 Obtaining an Initial Guess 

3.2.2 Simple Solution with Dynamic Pressure Constraint] 

3.2.3 A Penalty Function to Smooth Out Singular Jitter 


3 

4 

6 

7 

11 

12 

13 

18 

20 

24 

25 

25 

28 

30 

33 

35 

36 

39 

42 

48 


1 Introduction 


This document describes the user interface and provides a usage tutorial for the MADS (Min- 
imization Agent for Dynamical Systems) trajectory optimization tool. MADS comprises a 
FORTRAN95 subroutine that organizes and executes trajectory optimization computations, 
and a set of Matlab functions that manipulate MADS input and output data. MADS casts 
a trajectory optimization problem as direct cost function minimization subject to a set 
of constraints that include a temporal discretization of the first-order ordinary differential 
equations that govern the user’s plant dynamics, boundary conditions, and miscellaneous 
constraints imposed on the trajectory between boundary conditions. The resulting nonlin- 
ear programming problem (NLP) is solved by the NLP software SNOPT [1], which must be 
available in order to run MADS. 

The user provides four problem-specific subroutines that realize the system state rates, 
the boundary conditions, the trajectory path constraints, and the cost function. Given 
the maturity and benefit of autodifferentiation (AD) software, MADS assumes the presence 
of autodifferentiated versions of the four user routines to supply derivatives. MADS’ user 
subroutine interfaces assume the use of the TAPANADE [2] AD package. 

The approach taken in MADS to posing and solving trajectory optimization problems 
is to encourage robust convergence to a solution by using a low-order discretization and 
a control parameterization which can gracefully accommodate the temporal discontinuities 
which can appear on the interior of trajectory arcs when exploring an optimal control 
problem. The low-order discretization and control is normally accompanied by a fairly fine, 
uniformly distributed, mesh of time points over which the problem is solved. 

This emphasis on simplicity and accommodation of nonsmooth control behavior places 
MADS somewhat out of the mainstream of emerging numerical optimal control technology, 
much of which has been emphasizing pseudospectral techniques [3] - high-order quadra- 
ture of orthogonal polynomials, with adaptive temporal discretization mesh logic. These 
emerging techniques show very good performance and high accuracy on temporally smooth 
problems, but graceful treatment of nonsmooth behavior remains an area of active research. 

Section 2 describes the family of optimal control problems that can be solved with 
MADS, and describes its software interface and that of the user-supplied routines. It also 
describes the Matlab utility functions that are used for manipulating MADS initial guesses 
and solutions. 

Section 3 sets up and solves two simple, classic optimal control problems, both of which 
exhibit nonsmooth temporal behavior issues, several different ways. Simple measures are 
described for varying the discretization density over the trajectory, and for constraining 
aspects of the control trajectory’s behavior. 
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2 Problem Formulation and Software Interface 

MADS operates on trajectories of the form 
dxfc 

-^ = f(x k ,u k ,p), t £ [0, 1], k= l,...,nph (1) 

that is, for trajectories which may (when nph > 1) consist of multiple subarcs, or “phases.” 
In each k th phase, x k £ TZ nXk are states, and p £ lZ n? is a vector of “static” free parameters; 
that is, parameters to be chosen by the optimization process, and which do not vary with 
time. Note that while x k and u k are specific to the k th phase in 0, all of the elements of 
p are visible to all nph phases of the trajectory. Each u k (t) is assumed to be an integrable 
mi-dimensional functional defined on the interval t £ [0,1]. The integrability assumption 
is more-or-less moot, since MADS recasts the problem in discrete time. It is, however, the 
case that if the user attempts to solve a problem for which an integrable optimal solution 
does not exist, numerical difficulties will ensue. Subsection 2.2 of the Tutorials gives an 
example where optimal control leads to a nonintegrable “optimal” control solution, and 
gives approaches for compensating for the nonintegrability. It should also be noted that 
the unity duration of the subarcs in <[T[) does not restrict the user from posing variable-time 
problems. There are a number of easy ways to do this, and literally all of the examples in 
the Tutorial involve free terminal time. The very simplest is to use an element of p to scale 
time. Restricting to nph = 1 and p scalar for simplicity, transform f to r via r = pf so that 

flT 

=pf{x,u,p), t £ [0, 1] (2) 

Alternatively, an element, say u Tl of u can be used as a time scaling parameter; that is, 
r(f) = u T (t), giving 

= u T f(x,u,p), t £ [0, / u T (t)dt], u T > c T > 0 (3) 

t=u t (t) ^0 

where c T is a user-specified constant. This latter formulation permits the flexibility of vari- 
able time steps, at the cost of some additional complexity. The Tutorial includes problems 
using both approaches. 

Free parameters in the trajectory - state boundary values, control functionals, and the 
p vector are chosen to minimize a Mayer-type cost function 


dx 

dr 


(j){x W , X\ y , X20 1X2 f 1 • - - , ^nphO; ^nph / i p') • 


X k 0 = x k (0) 

Xkf = X k {l) 


(4) 


The cost function cf> is minimized subject to a discretization of 0 and, optionally, boundary 
conditions 


I , s f = 0, iebcvec(j) = 0 . , , 

V’i(*io,a:i/,.--,*npho,*nph/,P)| >0) iebcvec(j) = 1 J =1 >---> nbc ( 5 ) 

where nbc is the dimension of ip and iebcvec(j) is a user-specified flag that controls 
whether the j th element of 0 is to be treated as an equality or inequality constraint. Again 
optionally, constraints can be imposed on the trajectory between boundary conditions using 
a discretization of trajectory constraints of the form 

, J =0, iecv(j) = 0 . 

Cj(x k ,u k ,p) | > 0 iecv (j) = 1 k = 1, . . . , nph, j = 1, . . . , nc fe (6) 

where iecv is a user-specified flag. Note that the number of trajectory constraints in each 
of the nph subarcs, or “phases” may vary from phase to phase. 
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MADS computes optimized trajectories by direct minimization of the cost function <j>, 
using finite-dimensional approximations of the state and control trajectories, the former ob- 
tained via low-order collocation. Each k th trajectory arc in (|l| is broken into nd^ equal time 
subintervals and the state trajectory across each subinterval is approximated by collocation: 

Xj+i - Xj = F(xj,x j+1 ,Uj,p), j = l,...,nd fc (7) 

where Uj is constant, i.e., zero-order hold (ZOH) across the discretization intervals. Note 
that, while each k th discretized phase has one value Uj per discretization interval, for a total 
of ndj, values, there are nd^ + 1 corresponding values of Xj, since each phase has an initial 
and terminal state. 

The overall organization of constraints in MADS is 


Z = 


21 







2/i 


F^ 


, Zj — 


? Vi — 

[ Cij \ 

^riph 

TP 


Uiidj 



(8) 


where Fij is the discretization (p) for the i th time step of the j th phase, and Cjj is the 
corresponding constraint from (I6j). 

There are currently three discretization expressions implemented, and the MADS input 
parameter kode=kodev(k) controls which of them is used to discretize the k th phase of the 
trajectory: 


1. Midpoint Euler (ME) (kode=0) 


x i + 1 - x i = ^ + 2 J+1 ’ U FP S ) ( 9 ) 

This is the most efficient discretization, since it achieves second-order accuracy with 
only one state derivative computation per time step. 

2. Second-Order Runge-Kutta (RK2) (kode=l) 


X 3 + 1 

= Xj + ( 1/4) (7?! + 3%) '{ 


?7l 

= (1/nd) / (xj , Uj , p) \ 

(10) 


= (l/nd)/(xj + (2/3 )t7i,Mj,p) J 



This Runge-Kutta discretization requires twice as many state derivative computations 
as the ME, but has the property that the discretization is not dependent on Xj+ 

3. Fourth-Order Runge-Kutta (RK4) (kode=2) 


x j + 1 ~ 

(1/6) (771 + 2ij 2 + 2ij 3 + 774) 

Vi = 

(1/nd )f(xj,u,j,p) 

m = 

(l/nd) f(xj + (l/2)r ]1 ,Uj,p) 

m = 

(1/nd )f(xj + {l/2)rj 2 ,Uj,p) 

m = 

(l/ nd )/(^ +V3 ,Uj,p) 


( 11 ) 


This RK4 requires four times as many state derivative computations as the ME, but 
has a clear advantage in the fourth-order accuracy with which it discretizes the state 
trajectory. 


5 


2.1 Excursus: Custom Problem Formulations in MADS 

At first glance, the user may balk at MADS’ apparent crudity, most clearly exhibited in 
the zero-order hold (ZOH) imposed on the control variable. As the user’s experience with 
MADS grows, however, this should mature into appreciation for MADS’ flexibility. The 
ZOH is actually more of a “feature” of MADS, rather than a limitation. MADS is primarily 
intended to be used with the ME discretization and relatively small time steps. The use of a 
low-order discretization and small time steps encourages robust convergence to the problem 
solution, and the small time steps allow the ZOH to closely approximate the continuous-time 
optimal control trajectory. 

All that being said, as stated in the Introduction, MADS is intended to provide a con- 
venient “blank sheet” environment in which the user can formulate a wide range of trajc- 
tory optimization problems without being particularly shackled by canned dynamical model 
structures, or presumptions about the temporal behavior of the control trajectory. 

In order to illustrate this, we consider adjustments to the standard MADS problem 
formulation that permit control trajectories more complicated than ZOH. Since the ME 
only samples the control trajectory at one point per discretization interval, why might a 
user want to complicate the control parameterization? Two reasons are to achieve higher 
numerical precision, or to obtain a control solution that accommodates known characteristics 
of the hardware that would actually realize the implement the control being optimized.. 

• The RK4 discretization may be used to check the validity of a ME-based solution 
by re-solving the problem on the set of time points used for the MEs solution and 
verifying that the RK4 and ME solutions are “sufficiently close.” Since the RK4 
samples the control trajectory at four points rather than one, fidelity to the continuous- 
time optimal control trajectory can be improved by choosing a more complicated 
control parameterization that varies over the discretization interval. 

• Optimal control trajectories are typically unrealistic in the sense that, while actual 
implemented controls are the response of finite-bandwidth actuators to commands, 
actuation dynamics are not typically included in plant models for trajectory optimiza- 
tion. The control parameterization examples described below all involve the use of 
state variables. With these in hand, the user can easily constrain or penalize unrealis- 
tic control features, such as “instantaneous” jumps that would require very expensive 
actuators to implement. The first solution example includes a demonstration of this 
type bandwidth-limited control. 

Returning to specifics, suppose, for example, that the user would prefer a piecewise linear 
control history, rather than the default ZOH. In that case, rewrite ([l]) as 

x = f(x,'y,p) (12) 

and define a state to propagate the control variable 7 (f): 

7 = u (13) 

and append it to the plant state x p i ant 

x = [ * 1 (14) 

L 7 J 

so that the value of piecewise constant u on the j th discretization interval is the slope of 7 
over that interval. If a discontinuous first-order hold (FOH) control variation is preferred, 
define a state to model time variation inside the discretization interval: 

it = 1, z t (0) = 0 (15) 
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and define the interval time as 


tj =x t -j/nd (16) 

where both j and nd are passed to the user’s plant dynamics subroutine through the calling 
argument. The FOH control, then, is 


7 j{tj) — c o j + c i jtj (17) 

where coj, c\j appear in the problem formulation as elements appended to the vector u p i an t 
that contains whatever controls, if any, are modelled as ZOH: 


r- n 


^plant 

*£plant 

, Uj — 

X t 

c 0 j 



. Cl i 


(18) 


If a high-order control variation with continous slope is desired, it can be simply imple- 
mented by constructing a Hermite-type spline. Again define a time state 


Xj = nd, i(0) = 0, tj = x t — j (19) 

so that tj passes from 0 to 1 over each j th interval. With tj in hand, define the control 
function as 


7. j{tj) ~ c 0 j + Cl jtj + C2jtj 

and require 


( 20 ) 


TjW = 77+1 (0) c 0 j + Cij + c 2 j - Coj +1 = 0 

TiW = 7 j+i Cij + 2c 2j - c hj+1 = 0 


( 21 ) 


These dynamical constraints require that Ci and C 2 be modelled as states added to the plant 
state vector: 


(ci )j — j 

(C2 )j = k 2 j 

and k\j and k 2 j are appended to the control vector Uj, resulting in 


(22) 


•tplant 


TTplant 

Xt 


CO 

Cl 

, u = 

ki 

C2 


k 2 


(23) 


The discussion above has been presented primarily to whet the reader’s imagination for 
posing MADS problems, and to provide assurance that it is not difficult to set up problems 
that fall outside the MADS defaults. 


2.2 Subroutines To Be Provided By The User 

The user’s problem, as expressed by is implemented in four user-supplied subrou- 

tines. These can be divided into two groups - subroutines that operate along trajectory arcs, 
and subroutines that operate on boundary values. Before providing individual details of the 
user-supplied subroutines, we describe a SNOPT-related input common to all of them. That 
input is an integer scalar, nstate, which is generated by SNOPT to provide the user with 
signals for initialization operations - such as data initialization and memory allocation - 
and post-run cleanup operations such as deallocation. The three most important conditions 
for nstate are 
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• nstate=0 

normal subroutine call, 

• nstate=l 

first call. If there are special operations to be performed on the first call, perform 
them now, then proceed on to the operations performed for nstate = 0. Note that 
there is only one nstate = 1 call to each of the user subroutines at the beginning of 
a MADS run. 

• nstate=2 

final call. If the user has cleanup operations to perform, they should be done now. 
MADS has finished, and will not use the results of any computations performed dur- 
ing this call, so it would be best to simply return once user cleanup operations are 
complete. If the user has nothing to do for the nstate = 2 call in any of the four user- 
supplied subroutines, simply make the first executable line of each of the subroutines 
“if (nstate . GE . 2) return.” 


There are several other values that nstate can take on, none of which are recognized 
specifically by MADS. Those values are explained in the SNOPT documentation [1], The 
user is recommended not use nstate, but rather to perform initialization and memory 
operations in code units separate from those implementing 04® 

The subroutines that operate along trajectory arcs are 


xdot: This subroutine corresponds to and provides the right-hand side of the system of 
first-order ordinary differential equations (ODEs) defining the trajectory for each of 
the nph trajectory phases; that is to say, MADS calls one xdot, and that subroutine 
has logic to return the state derivative for each k th phase, k = 1, . . . ,nph. The calling 
syntax for xdot is 


subrout ine xdot (nstate , kph , nk , j k , nx , x , nu , u , np , p , f ) 
integer , intent (in) :: nstate, kph, nk, jk,nx,nu,np 
real (8) , intent (in) :: x(nx) ,u(nu) ,p(np) 
real (8) , intent (out) :: f (nx) 


The inputs are 


nstate : 
kph: 
nk: 

jk: 

nx : 
x : 
nu: 
u: 
np: 
p: 


See the discussion at the beginning of this Subsection. 

index of current trajectory phase, i.e. 1 < kph < nph 

number of discretization intervals in this phase 

index number of current discretization interval, i.e. 1 < jk < nk 

state dimension in current phase 

nx-element state vector 

control dimension in current phase 

current value of nu-element control vector 

dimension of vector of static free parameter 

np-element static free parameter vector 


The output is 

f : right-hand side of the state time derivative, for the current inputs. 


cineq: This subroutine corresponds to ©b and provides the equality and inequality con- 
straints that operate on the state and control trajectories in between boundary condi- 
tions. This routine differs from xdot in that it has access to the state at the discrete 
instants at the beginning and end of the current discretization interval, whereas, xdot 
is simply called with whatever state argument is provided it by the user’s chosen ME, 
RK2, or RK4 discretization logic. This routine also differs from xdot in that the user 
needs to specify, for each element of the output vector, whether it is to be treated as 
an equality or inequality constraint. Recall that all inequality constraints in MADS, 
per ©© are posed so that they are satisfied by non-negative values. The calling 
syntax for cineq is 

subroutine cineq(nstate,kph,nk, jk,nx,xj ,xjpl,nu,u,np,p, & 

& nc , c , iec , iecf lag) 

integer , intent (in) :: nstate,kph,nk, jk,nx,nu,np,nc , iecf lag 
integer , intent (out) :: iec(nc) 

real (8) , intent (in) :: xj (nx) ,xjpl(nx) ,u(nu) ,p(np) 
real (8) , intent (out) :: c(nc) 


The inputs are 


nstate : 
kph: 
nk: 

jk: 

nx : 

xj : 

xjpl: 

nu: 

u: 

np: 

P = 
c : 

iecf lag: 


See the discussion at the beginning of this Subsection. 

index of current trajectory phase, i.e. 1 < kph < nph 

number of discretization intervals in this phase 

index number of current discretization interval, i.e. 1 < jk < nk 

state dimension in current phase 

nx-element state vector at the beginning of discretization interval jk 

nx-element state vector at the end of discretization interval jk 

control dimension in current phase 

current value of nu-element control vector 

dimension of vector of static free parameter 

np-element static free parameter vector 

the dimension of the constraint vector c that is to be output by cineq 

integer scalar that signals cineq to output the iec vector. The values of iecf lag 
that MADS inputs are 

0: a normal call, in which cineq is to compute its constraint quantities. 

1: cineq is required to output iec and then exit. No other operations are 
wanted from cineq in this case. If the user does perform other operations, 
they will be ignored. 


The outputs are 

c : nc-dimensional constraint vector from ^ 

iec: nc-dimensional integer vector of ones and zeros whose i th component signals 
MADS whether c(i) is an equality or inequality constraint. The values of iec are 

iec(i)=0: c{i) = 0 is required for solution 
iec(i) = l : c{i ) > 0 is required for solutions 
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The next two user subroutines operate on state boundary values and the p vector to im- 
plement @ and [5]). The boundary values are available to the routines as a vector, xbc, 
that stacks the initial and terminal values, along with integer vectors kO and kf such that 
kO(k) + 1 points to the first element of the initial state for the k th trajectory phase and 
kf(k) + 1 points to the first element of its terminal state, in turn. This is displayed in the 
following equation, in which refers to the first (initial boundary value) state vector in 
the k th phase and Xf & refers to the last (terminal boundary value) state vector in the k th 
phase. For a trajectory with nph phases, there will be nph state dimensions, which can be 
collected in an integer vector nxv(k), k = 1 , . . . ,nph: 


xbc = 


£ 0,1 

...kO(l) + l 

*/, 1 

. . . kf (1) + 1 Zo.fc = 

x 0,2 

. . ,k0(2) + 1 

X f, 2 

. . . kf (2) + 1 L 

^0,nph 

. . . kO(nph) + 1 x f,k = 

•£/,nph 

. . . kf (nph) + 1 


(£o,fc)i 


( a: 0,fe)nxv(k) 


(£nd fc + l,fc)l 


( x nd k -f- 1 , k ) nxv(k) 


(24) 


The two user subroutines that operate on xbc and p are psibc for and phiobj for the 
cost Q: 


psibc As noted above, this subroutine implements operating on the trajectory’s bound- 
ary values and the p vector to compute boundary conditions as equality or inequality 
constraints, as specified by the user. The calling syntax is: 


subroutine psibc (nstate, nph, kO,kf,nxv,nxbc, xbc, np,p, & 

& npsi ,psi , iebc , iebcf lag) 

integer , intent (in) :: nstate, nph, kO (nph) ,kf (nph) ,nxv(nph) , & 

& nxbc,np,npsi,iebcflag 

integer , intent (out) :: iebc(npsi) 

real (8) , intent (in) :: xbc(nxbc) ,p(np) 

real (8) , intent (out) :: psi(npsi) 


The inputs are: 


nstate : 
nph: 
kO: 
kf : 
nxv : 
nxbc : 
np: 
npsi : 
iebcf lag 


See the discussion at the beginning of this Subsection, 
the number of phases in the trajectory 


explained in (24 1 


explained in (24 1 

nph-element integer containing state dimension for each phase 
dimension of xbc, that is 2* sum (nxv) 


dimension of p vector 

dimension of psi vector - number of boundary conditions 

integer scalar that signals psibc to output the iecbvec vector. The values of 
iebcf lag that MADS inputs are 

0: a normal call, in which psibc is to compute its constraint quantities. 

1: psibc is required to output iecbvec and then exit. No other operations are 
wanted from psibc in this case. If the user does perform other operations, 
they will be ignored. 
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The outputs are: 

psi : the npsi-element vector of boundary condition constraints 

iebc: np si-dimensional integer vector of ones and zeros whose i th component signals 
MADS whether ij)(i) is an equality or inequality constraint. The values of iebc 
are 

iebc(i)=0: ijj(i) = 0 is required for solution 
iebc(i) = l : ip(i) > 0 is required for solutions 

phiobj : This routine implements Q, evaluating the cost function (j). The calling syntax is 

subrout ine phiobj (nst at e , nph , kO , kf , nxv , nxbc , xbc , np , p , phi ) 
integer, intent (in) :: nstate,nph,kO(nph) ,kf (nph) ,nxv(nph) , & 

& nxbc,np 

real (8) , intent (in) :: xbc (nxbc) ,p(np) 

real (8) , intent (out) :: phi 


The, by now, familiar inputs are: 

nstate: See the discussion at the beginning of this Subsection, 
nph: the number of phases in the trajectory 
kO : explained in ( 24 1 


kf : explained in ( 24 1 

nxv : nph-element integer containing state dimension for each phase 
nxbc: dimension of xbc, that is 2*sum(nxv) 
np : dimension of p vector 

and the subroutine outputs the cost: 

phi : the scalar cost function 


2.3 Producing and Operating On A MADS Solution 

This Subsection describes the organization of data in a MADS solution and describes soft- 
ware for operating on that data. The software is a mix of FORTRAN95 for the actual 
optimization calculations, and Matlab functions for data manipulation. This Subsection 
does not go into detail in formulating and coding trajectory optimization problems; that is 
provided via solved examples in Section 2. 

The steps in producing a MADS solution are as follows: 

1. Formulate a reasonable problem. 

2. Code the four user-supplied subroutines, per Subsection 1.2. 

3. Code routines to provide the derivatives of the four subroutines referred to in Step 
2. This is to be done using autodifferentiation software; automatic, widely available, 
reliable, and (currently) free for noncommercial use. 

4. Assemble an initial guess. 

5. Run MADS. 
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6. Review the solution. Accept it, or modify the problem formulation and go to Step 2. 


Steps 1 and 2 are up to the user, with help and insight from this Tutorial. Step 3 can 
be accomplished using scripts provided with the MADS package, provided that the user 
installs TAPANADE [2]. This Section will also provide generic comments on autodiffer- 
entiation for MADS, should the user prefer to use a different package. Before discussing 
autodifferentiation, though, we will concentrate on Steps 4 through 6. 


2.3.1 Formats for MADS Solution Data 


We first consider the organization of data in MADS, and software for manipulating it. The 
problem variables for optimization are stacked together in a vector: 



*i 

*2 


*^1 ,k 
Ui t k 

vmads = 

^nph 

P 

Vk = 

^ndfc ,k 
^ndfc , k 
^ndfc + lj/c 


(25) 


In order for MADS to operate on Vmads > it requires dimensional and program option data. 
This is supplied by a text file with integers as follows: 


nph, 

npsi, 

np 



nxv(l), 

nuv(l), 

ncv(l), 

ndv(l), 

kodev(l) 

nxv(nph), 

nuv(nph), 

ncv(nph), 

ndv (nph), 

kodev(nph) 


This will be referred to in the sequel as a “premads” file. 

Because ( 25 1 and ( 26 ) comprise a miserably inconvenient format for operating on or visu- 
alizing a MADS solution, utility functions are provided for converting between vmads / premads 
and a Matlab structure - call it “5”’ - with the following fields: 


S . nph : number of phases 
S . np : dimension of p 

S . nxv : nph-element array of state dimensions 
S . nuv : nph-element array of control dimensions 

S . ndv : nph-element array whose k th element is the number of discretization intervals in the 
k th phase. 

S.x: nph-element cell array containing whose k th element contains that phase’s state tra- 
jectory.. Each x{k} is 


x{k} = 


* 1,1 

* 2,1 


*1,2 

*2,2 


*^l,nxv(k) 

*^2,nxv(k) 


*£(ndv(k) + l),l *£(ndv(k) + l),2 * ' ' ^(ndv(k)+l),nxv(k) 


(27) 
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S . tx : nph-element cell array containing time (independent variable) instants for plotting the 
trajectory in S.x. These assume the use of ([2]) for time scaling, and that the first nph 
elements of the p vector are used for this purpose. The organization of S.txjk} is 


S.txjk} 


t 0 (k) 

t-o(k) + p(fc)/ndv(k) 



t 0 {k) +p (k) 

0 , 

to(k - 1) + p (k - 1), 


k = 1 
k > 1 


(28) 


S.u: nph-element cell array whose k th element contains that phase’s control trajectory. 
Similarly to S.x, S.uk has the structure 


« 1,1 

Ml, 2 

^l,nuv(k) 

«2,1 

U2,2 

^2,nuv(k) 

^ndv(k),l 

^ndv(k) .2 

^ndv(k) ,nuv(k) 


Note that there are ndv(k) instants in this array, rather than ndv(k)+l. 


(29) 


S . tu : nph-element cell array containing time instants for plotting the control trajectory in 
S.u. Again, the assumption is made that ([2| is used; but the control instants are 
indexed to the midpoints of the discretization intervals: 


S.tujk} 


to(k) + |p(k)/ndv(k) 
t 0 (k) + (1 + |)p(k)/ndv(k) 

t 0 (k) + (ndv(k) — |)p(k)/ndv(k) 


(30) 


and to (k) is defined in ( 28 1 . 

A nice feature of this structure is, of course, that the user can add additional fields to S. 


2.3.2 Matlab Functions for Operating On MADS Data 

The MADS package includes several Matlab functions for 

• importing of Vmads data into the Matlab environment, and supporting its visualiza- 
tion, 

• modifying MADS solution data for a given problem formulation, in support of con- 
structing an initial guess for an alternate, but related, problem formulation, 

• exporting S-format data to MADS input data. 

The functions that import MADS data and support visualization are, primarily, plotmadsMADS, 
and dtplotMADS. Two additional functions, getMADS, readpremadsMADS are included to pro- 
vide the user with a little more flexibility in programming style. The functions are 

importMADS: This is the main import function for MADS, and is called as 
S=importMADS (premads , f data , flag) 
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The inputs are: 

premads : character string, the name of the premads file for the MADS data to be imported. 

fdata: another character string, this the name of the MADS output data file, whose 
data is in vmads format. 

flag: scalar flag to indicate whether or not the lagrange multipliers computed by 
SNOPT for the MADS problem should also be imported. The admissible values 
are 

0 . . . don’t import 
1 . . . do 

The importMADS output is S, which contains all of the data in Subsection 1.3.1 and, 
additionally, 


S . up : nph-element cell array containing the control trajectory, formatted to plot as a 
sequence of zero-order hold values. 

S.tup: nph-element cell array containing the corresponding time values, again, assuming 
that the first S.np element of the S.p vector are used in ([2]) for time scaling. 

S . lamx : cell array containing lagrange multiplier histories for the discretization constraints, 
output if flag=l. 

S . lama : cell array containing lagrange multiplier histories for the constraints in © , output 
if flag=l. lamak=[] for those phases where there are no Q constraints. 

S . lamp si : lagrange multiplier vector for © , output if f lag=l. Note that lampsi= [] if there 
are no boundary conditions. 


It should also be noted that, if S.np = 0, then S.p is returned as nan. 

dtplotMADS : This function produces time vectors for use in plotting S state and control trajectories 
for the case where ([3]) is used for variable time steps. It is called as 


[tu,ux] =dtplotMADS(dtin) 


with input 

dtin: nph-element cell array, the k th element of which contains the vector of u T from 
the MADS solution for that phase. 

The outputs are 

tu : cell array containing the time values for plotting S . u. 
tx : cell array containing the time values for plotting S . x. 

The user who has a solution with variable time steps should get the solution with 
importMADS, then discard that S . tx, S . tu, and run [S . tu , S . tx] =dtplotMADS (dtin) , 
having pulled dtin together from the S.u data. 

getMADS : This function, called as 


S=getMADS (fdata , nxv , nuv , ndv , np) 


outputs an S data structure containing only x, u, tx, tu, p, nxv, nuv, ndv, and np. 
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readpreMADS : This function, called as 
S=readpreMADS (name) 

reads a premads file with name name and outputs a partially populated S structure, 
which contains the dimensional and discretization option information contained in the 
premads file. 

The Matlab functions that support modifying and exporting MADS data are adduxMADS, 
breaktrajMADS, dtaunphMADS, and writepreMADS. 

adduxMADS : This function, called as 

vout=adduxMADS(x,u, nxout ,nuout .randmag, (optional) outind) 

pads or removes columns from S'- format x and u, and collects them into a Vmads~ 
format vector. The states and control may optionally be reordered before concatenat- 
ing into Vmads ■ The inputs are 

x: S-format state trajectory x(nd+l,nx) 

u: S-format control trajectory u(nd,nu). This may be empty if nu = 0. 

nxout : desired output state dimension. If nxout > nx, then the additional states are 
indexed as x(nx + 1) . . . x(nxout). 

nuout : desired output control dimension. Padding for u follows the same pattern as with 

x. 

randmag: If nxout > nx or nuout > nu, the padded extra states or controls are popu- 
lated with r=randmag*rand, where rand is the Matlab uniform random number 
generator. 

outind: (optional argument) structure containing desired ordering of states or controls 
in the output. The x indices are in outind. xind and the u indices are in 
outind.uind. If either xind or uind are not to rearrange state or control in- 
dices, they may be set empty; otherwise, the dimension of each must be nxout 
and nuout, respectively. As a concrete example, suppose that nx = 3, nxout = 5 
and outind.xind=[4 1:35]. In this case, the S-format organization of each 
row of the expanded state is x^ = [x(4) x(l) fc x(2) fc x(3) fc x(5)], k = 1, . . . , nd. 

The output is 

vout : x and u in vmads format. 

Note that, for multiphase problems, adduxMADS is simply called for each phase, and 
the output vectors are stacked to make the full Vmads- For example, 

randmag=0 ; 
v= [] ; 
for k=l:n 

v=[v; adduxMADS (S.x{k},S.u-[k}, nxout (k) , nuout (k) , randmag)] ; 
end 

v=[v;S.p] ; 
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breaktrajMADS : This function is used to break a single xk, uk phase into subphases and, optionally, 
to resample one or more of the subphases with a different number of discretization 
intervals. The function also assumes that the time scaling approach of ([2]) is used, and 
produces a vector of phase durations for the output subphases. The function is called 
as 


bout=breaktrajMADS(x,u,bv,tau, (optional) ndin) 


The inputs are 

x: S'-format state trajectory x(nd+l,nx) 

u: S'-format control trajectory u(nd,nu). This may be empty if nu = 0 . 
bv : vector of breakpoints - time-wise indices comprising the initial points of the out- 
put subphases. Note, bv does not include the inital point of the input single- 
phase trajectory, i.e. bv 7^ 1 . The breakpoints bv are defined in terms of 
the state discretization mesh points, with bv = k breaking the trajectory at 
mesh point k. Suppose, for simplicity, we have x scalar, nph=l, ndv=20, and 
bv=[ 3 ]. This breaks the trajectory into two output phases, with ndv=[2 18 ], 
and Xh ouf (kf(l) + 1 ) = x( 3 ) and Xf, o1it (k 0 ( 2 ) + 1 ) = x( 3 ) , where Xf , out is informal 
notation for the multiphase output of breaktrajMADS. This, and the fact that 
controls are defined on the interval between the k th and ( k + l) th instants means 
that if the user uses the control trajectory to choose a breakpoint, the chosen 
control instant will appear in the phase to the right of the breakpoint. Note that 
it is admissible to input bv= [] . This would correspond to the case of resampling 
a single trajectory phase. In this case, ndin, detailed below, would have a single 
element. 

tau: scalar - the duration of the input phase, 
ndin: This is an optional input, but must be present if bv=[] . ndin is the number of 
integration intervals desired for each of the output subphases. The resampling is 
done using piecewise linear interpolation of input state and control trajectories. 

and the output is 

bout : structure contining the split trajectory and associated information: 

ndv: nbv-element array containing the number of discretization intervals in each 
subphase. 

tau: nbv-element array containing the duration of each subphase, based on the 
input value of tau. 

x: nbv-element cell array containing state subphases. Assuming that ndin is 
not input, we have (referring to bout . x as x and bout . nph as nph, 




Xl 



x{l} = 





^bv(l) 


Xl 



^bv(k— 1 



—> < 

x{k} = 



X-nd+l 



^bv(k) 

k = 2, 

- 



•^bv(nph) 

-1 


xjnph} = 





Xnd+ j 



( 31 ) 
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u: nbv-element cell array of control subphases. Here, 



/ 

Ul 



U{1} = 





Ubv(l) 



Ul 



u bv(k-l 




— > < 

u{k} = 




. U nd _ 



Ubv(k) 

k = 2, 

- 



Ubv(nph) 

-1 


ujnph} = 




< 

Ujid 



, nph — 1 


dtaunphMADS : This function, called as 


(32) 


dtout=dtaunphMADS (bv , dt in) 

breaks the u T trajectory from (|3| into multiple subphases. It needs to be used in order 
to correctly scale the subphases’ u T s. The inputs are 

bv: vector of breakpoints, identical to that for breaktrajMADS, above, 
dtin: vector of u T values taken from a MADS solution. 

and the output is 

dtout : cell array of with length (bv) + l elements. Each k th cell contains the u T trajectory 
for the subphase of dtin that began with its element bv(k). 

writepreMADS : This function is the twin of readpreMADS. It inputs an S structure and writes the 
corresponding premads file onto a file with name name. It is called as 


writepreMADS (S , name) 


Function writepreMADS opens and closes file “name.” 

There are several other MADS support functions with very specialized applicability, and 
they will be described in their contexts. Before leaving this topic, though, there is one 
additional function to describe. 

findIMADS: This function can be used to assist in debugging MADS problems. The function is 
called as 


thel=f indIMADS (S , row) 

with S structure S and “row” as inputs. This function is used to help diagnose mis- 
behavior in MADS solutions. The diagnostic output from SNOPT includes a check of 
the correctness of the overall problem jacobian at the beginning of the run, comparing 
the user-supplied analytic jacobian to one using numerical differentiation. If there’s 
an error at a particular constraint element - “row” the comment “bad” is appended to 
the right of the output for that row. At the end of the run, a summary of the final con- 
straint activity is provided, labelled “Section 1 - Rows.” If there is an infeasibility 
for a particular constraint (row), it will be flagged with an “I” in the column labelled 
“State.” To use findIMADS, the user supplies dimensional data in S, gotten either by 
using importMADS or readpreMADS, and the offending row number. The output is 
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the I : structure containing 

kph: the phase number. thel.kph=[] if the constraint is in psibc. 

jk: the discretization interval, thel. jk=[] if the constraint is in psibc. 
xcp: the “type” of the constraint: ’c’ if in cineq, ’x’ if in xdot, or ’p' if in psibc. 
ind: the position of the constraint in the in the user-supplied routine’s output. 
For example, if thel . xcp= 1 p 1 and thel . ind=2, the problem is with psi (2) . 

We hope that the user will never want to use this function, but we know better. 

2.4 Setting Up and Executing a MADS Run 

Thus far, we have described the general MADS problem formulation, the user-supplied 
subroutines for realizing a given problem in MADS, and utilitiies for operating on MADS’ 
input and output data. We now turn to the mechanics of actually making a MADS run. 

There are three principal considerations to be kept in mind when setting up a problem 
for MADS: 

1. Because the problem formulation resides in the four separate subroutines of Subsection 
1.2, it is generally in the user’s best interest to treat those data that are common to two 
or more problem routines as global variables. We recommend use of the FORTRAN 
“MODULE” program unit as a means of safely sharing data across multiple local 
program units. Consider the following code fragment: 

module myprobMOD 
implicit none 

integer, parameter :: & 

& UsedByCineq = 1, & 

& UsedByAll = UsedByCineq + 2 
real(8) :: & 

& xdotParam 

end module myprobMOD 

subroutine cineq(nstate, kph, nk, jk, nx, xj , xjpl, & 

& nu, u, np, p, nc, c, iec, iecf lag) 

use myprobMOD, only : UsedByCineq, UsedByAll 
implicit none 


subroutine xdot(nstate, kph, nk, jk, nx, x, nu, u, np, p, f ) 
use myprobMOD, only : UsedByAll, xdotParam 
implicit none 


program myprob 

use myprobMOD, only xdotParam 

implicit none 


xdotParam = some number 
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2. Typically, for a given physical plant model, several different trajectory optimziation 
problems may be posed for it. These may simply be different missions that the plant 
is being called upon to perform, or it may be different formulations of superficially 
similar optimization problems that the user explores while seeking results that are 
not only optimal but desirable. Each of the examples in the Tutorial Section 2 will 
contain examples of this type of exploration. Because of this, it is highly desirable 
that the user strictly separate plant model software from problem formulation software. 
For example, suppose that a problem is posed with free terminal time using the © 
formulation. In that case, xdot should look like 

module FreeTimeMOD 

integer, parameter :: & 

& loctau = 1 ! location of terminal time in p 

end module FreeTimeMOD 

subroutine xdot(nstate, kph, nk, jk, nx, x, nu, u, np, p, f ) 
use FreeTimeMOD, only : loctau 

! MyPlant is a separate subroutine for plant ODEs 
call MyPlant(x, u, f ) 

f = p(loctau) * f ! Here is the time — scaling 
end subroutine xdot 

3. The instantiation of a given MADS problem will typically involve some 15-16 unique 
files, not counting those pertaining to the plant model. These include “plain” and 
autodifferentiated xdot, cineq, psibc and phiobj routines, main and MODULE pro- 
gram units, input and output data, and scripts for visualization and for assembling 
the initial guess. Experience teaches that, unless the problem-unique files for each 
MADS problem are kept separate from those of other MADS problems, confusion will 
ensue. The authors strongly recommend devoting a directory or “folder” to each such 
problem and, further, to affix a common character string to each of the names of each 
of the files that associates them with their particular problem. This practice will be 
illustrated in the Tutorial. 

MADS execution is performed by the subroutine batchMADS, calling SNOPT. This subrou- 
tine is called by a main routine, and needs to be linked to the MADS subroutine library, 
the problem subroutines cineq, xdot, psibc, phiobj, their derivatives cineq_dv, xdot_dv, 
psibc_dv, phiobj _dv, and any model-specific subroutines. Generation of the derivative rou- 
tines’ source code is described in the next Subsection, and the syntax of batchMADS is given 
below: 

batchMADS: This subroutine, 

subroutine batchMADS (f indata, f premads , f outdata, inform) 
integer, intent (in) :: findata,fpremads,foutdata 
integer, intent (out) : : inform 

has the following arguments: 

f indata: FORTRAN “unit number” for an opened file containing the input guess. 
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f premads : 
f out data: 


info : 


Unit number for an opened file containing the premads file for the given problem. 


Unit number for an opened file into which batchMADS will write the solution 
data at the conclusion of batchMADS execution. This file contains a concatena- 
tion of the output value of vmads from (25) and the SNOPT-computed lagrange 
multipliers for the constraints in the Z vector, defined in ([8]). Recall that ex- 
tracting and organizing the lagrange multipliers from MADS output is handled 
by importMADS as an option. If SNOPT fails to converge, the output data in 
foutdata corresponds to the state of the iterations at the end of the run. 


This is a batchMADS output, and is a pass-through from SNOPT, in whose doc- 
umentation [1] info is fully documented. The most typical values that will be 
encountered by the MADS user are 


1 : finished successfully 
3 : couldn’t achieve desired accuracy 
13: infeasible constraint (s) 

41 : current point cannot be improved 
52 : incorrect constraint derivatives 


In batchMADS, several SNOPT parameters are set. These are Infinite Bound set to 
10 20 , Verify Level, set to 3, Derivative Level, set to 3, and Linesearch Tolerance, 
set to 0.99. These values can be overridden, or other SNOPT options set by writing 
and linking a subroutine user set: 


subroutine user set (fprt , f summ, inf o , cw, lew , iw, liw,rw, lrw) 

integer , intent (in) :: f prt,f summ, lew, liw, lrw 

integer , intent (out) :: info,iw(liw) 

real (8) , intent (out) :: rw(lrw) 

character (8) .intent (out) cw(lcw) 


In the body of this subroutine, the user calls SNOPT routines snseti, or snsetr to 
set parameters as preferred. The user is not required to provide a userset, as the 
MADS library includes a dummy for linking. 

2.4.1 Autodifferentiation for MADS 

This Subsection provides a short discussion of algorithmic differentiation (AD) in MADS. 
Algorithmic, or “automatic” differentiation [4] is the use of software techniques to numer- 
ically evaluate the derivative of a function calculated by a computer code. This is - very 
superficially speaking - done by analytically differentiating the most primitive computations 
in the code, e.g., exponentiation, transcendental functions, and so on, and building up the 
full evaluation of the derivative by repeatedly applying the chain rule. This technique has 
a substantial advantage over the use of finite differences in that the computed derivative is 
accurate to machine precision, with no degradation due to the truncation that is associated 
with difference-based differentiation. 

MADS requires the derivatives of / 0, c 0’ 4> 0, and </> ©• If the user has 
TAPANADE [2] installed, the subroutines that provide these derivatives - xdot.dv, cineq_dv, 
psibc_dv, phiobj_dv - are simply obtained by executing the following Matlab script invo- 
cations from the command line: 

tapxdot ( 1 routines called by xdot , MODULES referenced by xdot ’ ) 
tapcineq( ’routines called by cineq, MODULES referenced by cineq’) 
tappsibc( ’routines called by psibc, MODULES referenced by psibc’) 
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tapphiobj ( ’routines called by phiobj , MODULES referenced by phiobj ’ ) 
tapscript 

and then the user need not think any further about AD. In the code fragments above, 
incidentally, the arguments to each of the tapxxx functions are to be input as string variables. 
For example, if xdot calls MyPlant.f90 and references variables from MyProbMOD . f 90, then 
the detailed call to tapxdot would be 

tapxdot ( ’MyProbMOD . f 90 MyPlant . f 90’ ) 

The order of files in the argument does not matter. Again, TAPANADE users can skip the 
rest of this section. 

If the user wishes to use a different AD package to generate the XXX_dv subroutines, it will 
need to be able to provide “multidimensional” differentiation, and to do so in “forward,” or 
synonymously, “linear tangent” mode. The first of these options will assure that the routines 
are differentiated over the entire span of their arguments: For a code that computes g{x) 1 
x £ 1Z n , rather than generating a _dv subroutine to compute v T g x , it will compute 

= fg (33) 

Regarding “forward” versus the alternative “adjoint” or, synonymously, “backward” modes 
of differentiation, “forward” is a differentiation mode that follows the order of execution in 
the subroutine(s) being differentiated. This mode produces subroutines with the interface 
syntax 

subroutine xdot_dv(nstate,kph,nk, jk,nx,x,xd,nu,u,ud,np,p,pd,f ,fd,nb) 
integer, intent (in) :: nstate ,kph,nk, jk,nx,nu,np,nb 

real(8) ,intent(in) : : x(nx) ,xd(nb,nx) ,u(nu) ,ud(nb,nu) ,p(np) ,pd(nb,np) 
real (8) , intent (out) :: f (nx) ,fd(nb,nx) 

! Note: nb=nx+nu+np 

subroutine cineq_dv (nstate, kph,nk, jk,nx,xj ,xjd,xjpl ,xjpld, & 

& nu,u,ud,np,p,pd,nc , c, cd, iec , iecf lag,nb) 

integer, intent (in) :: nstate, kph,nk,jk,nx,nu,np,nc,iecflag,nb 
integer, intent (out) :: iec(nc) 

real (8) , intent (in) :: xj (nx) ,xjd(nb,nx) ,xjpl(nx) ,xjpld(nb,nx) , & 

& u(nu) ,ud(nb,nu) ,p(np) ,pd(nb,np) 

real (8) , intent (out) :: c(nc) ,cd(nb,nc) 

! Note: nb=2*nx+nu+np 

subrout ine psibc_dv (nstate , nph , kO , kf , nxv , nxbc , xbc , xbcd , np , p , pd , & 

& npsi ,psi ,psid, iebc, iebcflag,nb) 

integer, intent (in) :: nstate , nph, kO (nph) ,kf (nph) , nxv (nph) ,np, & 

& npsi , iebcf lag, nb 

integer, intent (out) :: iebc(npsi) 

real (8) , intent (in) :: xbc(nxbc) , xbcd (nb, nxbc) ,p(np) ,pd(nb,np) 
real (8) , intent (out) :: psi(npsi) ,psid(nb,npsi) 

! Note: nb=nxbc+np 

subrout ine phiobj _dv (nstate , nph , kO , kf , nxv , nxbc , xbc , xbcd , np , p , pd , & 

& phi,phid,nb) 

integer, intent (in) :: nstate , nph, kO (nph) ,kf (nph) , nxv (nph) ,np,nb 
real (8) , intent (in) :: xbc (nxbc) , xbcd (nb, nxbc) ,p(np) ,pd(nb,np) 
real (8) , intent (out) :: phi,phid(nb) 

! Note: nb=nxbc+np 
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The _dv subroutines above are autodifferentiated with the following lists of dependent and 
independent variables: 


xdot 

cineq 

psibc 

phiobj 


dependent 

f 

c 

psi 

phi 


independent 
x u p 

xj xjpl u p 
xbc p 
xbc p 


TAPANADE, and other AD programs within the author’s experience, concatenates lists of 
multiple independent variables, conceptually, into a column vector “b”: 


xdot 

cineq 

psibc 

phiobj 


b = [x 1 u T p], 
b = [xj T xjpl^ u r p 7 ], 
b = [xbc 7 p], 
b = [xbc 7 p], 


nb = nx + nu + np 
nb = 2nx + nu + np 
nb = nxbc + np 
nb = nxbc + np 


(34) 


The “d”-suffix variables in the subroutine input arguments, e.g., xd, ud, pd in the case of 
xdot_dv, are column partitions of db/db T = I. Note the following: 


1. The orders of variables in (34 1 is not important. What is illustrated here is merely 
the order in which the variables are stacked in the MADS code. 


2. It is critical that the AD user autodifferentiates using the entire list of independent 
variables at once. For example, if a user autodifferentiates xdot first with dependent 
variable x, then with u, then with p, he or she might get a differentiated subroutine 
whose argument list looks superficially similar to that displayed above; but attempting 
its use would almost certainly crash the program’s execution or, worse yet, only provide 
erroneous results. 


3. It is not unusual for an AD code to presume that the dimensionality of b is actually 
larger than that declared via the list of dependent variables declared to the AD soft- 
ware. In order to accomodate this presumption, they do things such as introducing a 
global variable into the _dv code that contains the “real” row dimension of the d-suffix 
variables. For example, TAPANADE inserts the line USE DIFFSIZES into the sub- 
routine header, where DIFFSIZES is to be a user-supplied MODULE file containing a 
declaration of the variable NBDIRSMAX. This NBDIRSMAX is then used as the row dimen- 
sion for all of the d-suffix variables and certain intermediate variables. This behavior 
is unsuitable for MADS. The row dimensions of these variables should be nb, not 
NBDIRSMAX. This, and a few other annoyances are corrected for TAPANADE-based 
MADS users by executing the script tapscript. 

4. Some AD packages, by default, second-guess the user’s declaration of dependent vari- 
ables and fail to provide a d-suffix arguments in the _dv routine’s argument list for 
a given dependent variable if the declared dependency doesn’t actually exist for the 
given subroutine. For example, suppose that xdot didn’t actually include any element 
of the p vector in its computation of f . In this case, the AD output would have first 
line 


subroutine xdot_dv(nstate,kph,nk, jk,nx,x,xd,nu,u,ud, & 

& np,p,f ,fd,nb) 

This example is missing “pd,” and attempting its use in a MADS execution would 
result in, at best, a program crash, since MADS requires a particular, fixed, interface 
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to its _dv subroutines. The user should be wary about this. This can happen; for 
example, it is TAPANADE’s default behavior, though that can be corrected by using 
the command line option “-f ixinterf ace.” 
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3 Tutorial Examples 

This section is a tutorial for the use of MADS in solving numerical optimal control problems. 
The exposition is cast in the form of a sequence of example problems. This sequence 
progresses from very simple to fairly complicated, and each problem in the sequence features 
one or more tricks that are generically useful in other problems that the user may face. Two 
examples are considered: 

1. Linear System Minimum Time to Origin 

The optimal solution of this classic problem, described in Chapter 3.9 of [5] in continu- 
ous time is a “bang-bang” control trajectory in which the control jumps discontinously 
between maximum and minimum constraint limits. Because MADS employs a fixed 
time step in its discretization, it will be seen that the solution to the most straight- 
forward MADS formulation differs from the continuous time solution by having an 
“excrescence” in its control trajectory - a single time interval in which the control 
takes on an intermediate value. Two approaches for capturing the structure of the 
continuous time problem are described: 

• Break the problem into two phases. 

• Introduce variable discretization step size. 

Alternatively, recognizing that true “bang-bang” control trajectories are not physically 
realizable, we also demonstrate a technique that exploits the structure of the midpoint 
euler discretization to limit the bandwidth of the control solution by penalizing or 
constraining its rate and acceleration. An alternative to this technique would be 
to introoduce a low-pass filter for the control into the dynamics. In optimizing the 
trajectory with such a filter in place, however, there will be a tendency for the control 
solution to attempt to cancel out the filter dynamics. The approach given here has 
the advantage of operating directly on the control. 

2. Goddard Problem 

In the Goddard problem [8], the goal is to maximize the final altitude of a sounding 
rocket’s ascent. In the problem treated here, it flies through an exponentially decaying 
atmosphere in an inverse-square gravitational held, and is subject to an inequality 
constraint on dynamic pressure. 

This problem is distinguished by having a “singular arc” as described in Chapter 
8 of [5]. A singular arc, in a variational optimal control problem, is a finite por- 
tion of the optimal trajectory in which the control necessary condition for optimality 
vanishes identically; in other words, to first order, the optimal performance of the 
system is oblivious to the value of the control while it traverses the singular arc. It is 
straightforward to obtain converged solutions to such problems using MADS, but this 
insensitivity of performance to the control along singular arcs can affect the quality of 
the converged solution in two ways: 

(a) The user is likely to get a messy-looking control trajectory where there is no 
numerically unambiguous optimum. 

(b) The ambiguity in the control solution may also imply that the state trajectory 
drifts some from the exact optimum. 

In this example, a straightforward direct solution for the case without an active dynamic 
pressure constraint displays the “messy-looking” control trajectory warned of above, and a 
penalty function is applied to the jitter. The example concludes with imposition of a an 
active dynamic pressure inequality constraint. 
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3.1 Linear System Minimum Time to Origin 

3.1.1 Baseline Problem 

In this classic problem [5], a system of the form 


x = u u £ [—1, 1] (35) 

is driven from 

a;(0) = a i(0) = b (36) 


to the origin in minimum time, t*, resulting in an optimal trajectory in which the control, 
u(t), rides the saturation boundaries cited in (35 1, switching a maximum of one time from 
-1 to 1 or vice versa, depending on initial conditions. The problem is expressed for MADS 
by converting the expression x to first order ODEs: 


X\= X2 X 2 = u 

and time-scaling the the equations of motion, per ([2]), as 


x[ 




= r 


[ A \ 


U 


The cost function is 


(37) 


(38) 


(j> = T 

the control inequality constraints are 


(39) 


c(x , u) 


and the boundary conditions are 


1 — u 
1 + u 


> 0 


* 1 ( 0 ) — a 


' 0 ~ 

ar 2 (0) — b 


0 

x i (!) 

iebc = 

0 

x 2 (l) 


0 

t - e T 


1 


(40) 


(41) 


In (41 1, recall that boundary conditions expressed with iebc = 0 are treated as equality 
constraints. In the case where iebc = 1, above, the constraint is nonnegative: 


t — e T > 0 

where e T is a user-selected small positive number. A constraint of this kind is inexpensive to 
include, and can be quite valuable, since duration parameters in collocation-based trajectory 
optimization computations - at least in our experience - frequently exhibit unproductive 
behavior during the iterative search for a solution, taking on zero or negative values unless 
restrained. The parameter r is placed in the pv partition of the vector of free variables: 

vin — [^0: ^0; ^1; ' ' * •^nd: ^nd7 *^nd-f-l; 7"] 

where Xk = [(aq)fcj (%2 )fc] and nd is the number of discretization intervals selected. For this 
problem, we choose nd = 50. 

The subroutines below are reliably parsed by the version of TAPANADE [2] available at 
the time of writing. 
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subrout ine xdot (nstat e , kph , inst , xv , nx , uv , nu , pv , np , f v) 
implicit none 

integer, intent (in) :: nst ate, kph., inst, nx,nu,np 
real (8) .intent (in) :: xv(nx) ,uv(nu) ,pv(np) 
real (8) , intent (out) :: fv(nx) 
real(8) : : x2,u,p 

if (nstate . GT . Dreturn ! This is the last ‘ ‘cleanup 1 ’ call from SNDPT 

x2=xv(2) ! Calling out scalar variables aids in clarifying 

u=uv(l) ! complicated nonlinear expressions. Pointers could be 

p=pv(l) ! substituted here if they were reliably supported. 

fv(l)=x2 

fv(2)=u 

fv=fv*p 

return 

end subroutine xdot 

subroutine cineq(nstate ,kph,nx,xj , xjpl ,nu,uv,np ,pv,nc , cv) 
implicit none 

integer, intent (in) :: nstate , kph, nx,nu,np,nc 

real*8, intent (in) :: xj (nx) ,xjpl (nx) ,uv(nu) ,pv(np) 

real*8, intent (out) :: cv(nc) 

if (nstate . GT . Dreturn 

cv(l)=l+uv(l) 

cv(2)=l-uv(l) 

return 

end subroutine cineq 

subroutine psibc (nstate ,nph,kO,kf ,nxv,nxbc ,xbc ,np,pv,npsi , & 

& psi , iebc , iebcf lag) 

implicit none 

integer, intent (in) :: nstate ,nph,kO (nph) ,kf (nph) ,nxv(nph) , & 

& nxbc ,np ,npsi , iebcf lag 
integer, intent (out) :: iebc(npsi) 
real (8) , intent (in) :: xbc (nxbc) ,pv(np) 
real (8) , intent (out) :: psi(npsi) 
real(8) :: xl0,x20,xlf ,x2f ,tau 
if (nstate . GT . Dreturn 

if (iebcf lag. EQ. 1) then ! At the beginning of the run, tell MADS 

iebc=0 ! which boundary conditions are 

iebc (5) =1 ! equalities and which are inequalities, 

return ! and RETURN ! 

endif 

xlO=xbc(kO(l)+l) ! xl(0) 

x20=xbc(k0(l)+2) ! x2(0) 

xlf=xbc(kf (D+l) ! xl(t_f) 

x2f=xbc(kf (l)+2) ! x2(t_f) 

tf=pv(l) 

psi (l)=l-xlO 

psi(2)=l-x20 

psi (3)=xlf 

psi (4)=x2f 

psi(5)=tau-l .D-6 ! terminal time is positive 
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return 

end 


subroutine phiobj (nstate ,nph,kO,kf ,nxv,nxbc ,xbc ,np,pv,phi) 
implicit none 

integer, intent (in) : : nstate ,nph,kO (nph) ,kf (nph) ,nxv(nph) ,nxbc ,np 

real (8) , intent (in) :: xbc (nxbc) ,pv(np) 

real (8) , intent (out) :: phi 

if (nstate . GT . 1) return 

phi=pv(l) 

return 

end subroutine phiobj 

and, finally, the premads file is 


1,5,1 

2,1,50,2,0 

where the last element of the second line, recall, indicates that the midpoint Euler dis- 
cretization is specified. 

Having assembled software for the cost function and modelling constraints, and AD- 
processed the xdot, cineq, psibc, and phiobj routines, it remains to compute a converged 
solution. The first two attempts to compute a solution for this very simple, nearly linear, 
problem were unsuccessful. The initial guesses for vin were, respectively, all ones, and 
all random numbers generated by Matlab’s rand() function. These attempts were both 
unsuccessful - SNOPT was unable to compute a feasible iterate. 

The next attempt to generate an initial guess for this problem was to solve a similar, 
but less stringent, problem. The changes were to eliminate the state boundary conditions, 
moving them to the cost function. In other words, 

ij) = t — e, iebc = 1 

(jj = T + a * [(ah(0) - a) 2 + (x 2 (0) - b) 2 + (xi(l)) 2 + (x 2 {l)) 2 ] 

This was also an unsuccessful problem, in that it did no better in leading to a feasible 
solution. A successful initial guess was generated, however, by fixing r, rather than letting 
it vary freely. This was done by setting setting iebc to zero for the psibc constraint 
r — 1 = 0, and using all ones as an initial guess. 

This latter guess was passed on to the original problem formulation laid out above, and 
used successfully as an initial guess to obtain the time-optimal solution that satisfied the 
state boundary conditions. It will be seen that this pattern plays out generically in using 
MADS for solving OCPs. Initial guesses are most easily generated by relaxing path con- 
straints, such as boundary conditions, instead getting a solution time history that satisfies 
the discretization constraints, say ©>• Final time should be treated warily in the search for 
the initial guess. 

Figure [T] displays the solution for a minimum-time trajectory from initial conditions 
x(0) = 1, i(0) = 1. The plot on the right displays the actual control time history (piecewise 
constant) as dark red, and a continuous line drawn through the midpoints of the control 
increments in lighter red. As mentioned in the tutorial’s introduction, the control history 
is not perfectly “bang-bang,” having a little excrescence near the time t = 2.25 s. Two 
approaches for fixing this are given below: 


27 


dx/dt 


1.5 


1 

0.5 

0 


-0.5 


-1 


-1.5 



0 0.5 1 1.5 


X 


ZD 


1 r 


0.5 ■ 


0 ■ 


-0.5 


-1 


0 0.5 1 1.5 2 2.5 3 3.5 

time 


Figure 1. Two-State Bang-Bang Problem with Constant Time Step 


3.1.2 Break the problem into two phases 

A cleaner control discontinuity can be obtained by breaking the problem into two phases 
at the point at which the solution “bangs” from its maximum to its minimum value. This 
is done by introducing continuity boundary conditions on the states, including separate 
duration parameters for each phase, and setting the control to its appropriate constrained 
value during each phase. Denoting the states as xn, X 21 during the first phase and £12, *22 
during the second, the revised formulation is 


ill = * 2 l, *21 = U 

*12 = * 22 , *22 = U 


PHASE 1 
PHASE 2 


(xdot) 


(42) 


(j) = n + r 2 


(43) 


> n i PHASE 1 

u + 1 > 0 J 

U + 1 J j PHASE 2 
■u — 1 > 0 J 


(cineq) 


*n( 0 ) - a 


' 0 ' 

* 2 i( 0 ) - b 


0 

*u(l) - *i 2 ( 0 ) 


0 

*2i(l) - *22(0) 

iebc = 

0 

*12(1) 


0 

*22(1) 


0 

ri-e 


1 

e 


1 


(psibc) 


(44) 


(45) 
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Note that the inequalities in (44 1 enforce u = 1 in phase 1 and u 
psibc code fragment implementing the boundary conditions in (45 1 is 


1 in phase 2. A 


xllO=xbc(kO(l)+l) 
x210=xbc(k0(l)+2) 
xllf=xbc(kf (1)+1) 
x21f=xbc(kf (l)+2) 
xl20=xbc(k0(2)+l) 
x220=xbc (kO (2) +2) 
xl2f=xbc(kf (2)+l) 
x22f=xbc(kf (2)+2) 
taul=pv(l) 
tau2=pv(2) 


psi(l)=xllO-a 

psi(2)=x210-b 

psi(3)=xllf-xl20 ! continuity across phases 

psi (4)=x21f-x220 ! continuity across phases 

psi(5)=xl2f 

psi (6)=x22f 

psi (7)=taul-epsilon 

psi (8)=tau2-epsilon 

The initial guess for this problem is assembled from the solution of the single-phase prob- 
lem by using the MADS Matlab data utilities. Assume that the single-phase output data file 
is “bangbanglout . dat” and that the corresponding premads file is “bangbanglpremads . dat,” 
and that the trajectory’s control discontinuity occurs near the 27 th discretization interval. 
The code fragment for generating the input data for the two-phase problem is 

S=importMADS ( 1 bangbanglpremads . dat ’ , ’ bangbanglout . dat ’ , 0) ; 

°L tau — the duration of the single phase — is needed to 
7, compute durations of the two new phases. In this case, the 
7o only element of S.p is tau. Note that, when there are 
7, other static parameters than ‘‘tau,’’ it is convenient to 
7o place the trajectory duration(s) at one end of S.p or the 
/ other . 

tau=p (length (S.p)) ; 
bout=breaktraj MADS (S . x , S . u , 27 , tau) ; 

/ Overwrite various fields of S with two-phase data and 
"/, write a new premads file. 

S.p=bout .tau; 

S.x=bout .x; 

S . ndv=bout . ndv ; 

S . u=bout . u ; 

7, The user is required to supply the following information for 
7 0 constructing the premads file for the new problem. Note that 
7, the new state, control, and trajectory constraint dimensions 
'/. are identical with the old ones. 

S . nph=2 ; 

S.nxv=S.nxv*ones(2,l) ; 

S.nuv=S.nuv*ones(2,l) ; 

S.ncv=S.ncv*ones(2,l) ; 

S.kodev=[0;0] ; 
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S.npsi=8; 

writepreMADS (S , ’ bangbang2premads . dat ’ ) ; 

7, Generate and write the input guess file for the two-phase 
7o problem using adduxMADS, but without adding any additional 
7, states or controls. 

v= [] ; 

for k=l:S.nph 

v= [v ; adduxMADS (S . x{k} , S . u{k} , S . nxv (k) , S . nuv (k) , 0) ] ; 
end 

v= [v ; S . p] ; 

save bangbang2in.dat v -ascii -double 


3.1.3 Introduce variable discretization step size 

In order for the bang-bang control to be perfectly realized in the foregoing, it was necessary 
to break the problem into two phases, requiring an estimate of the point along the trajectory 
at which to position the break, and a substantial increase in the complexity of the psibc 
subroutine. 

This can be avoided by allowing the integration intervals in the discretization to vary 
along the trajectory so that the duration of the subarc with max control will naturally be 
“n” and that with minimum control will be “ 72 ”. Despite the fact that MADS nominally 
uses a fixed-step discretization, this can be achieved posing the time step as a control 
variable, rather than a scalar parameter: 


so that 


x' = u T (s)x{s), 0 < s < 1 


(46) 


r = 



(47) 


The increments u T are prevented from behaving irresponsibly by penalizing deviation of u T 
from its average value, 


Additionally, in cineq, impose 



(48) 


u T > c T > 0 (49) 

where c T is a user-selected constant. There are three things to be noted in this case: 


1. Although the plant dynamics are scaled by u T , in this case the elapsed time and 
penalty computations must not be. 


2. The duration r is the terminal boundary value of the differential equation for (47 1. 
This can be made to appear in the integral by imposing a boundary condition on r 
and a free static parameter, say, p T , in psibc: 


Pt = t 


so that (48 1 would be expressed 


(50) 


b T — (u r Pt) 


(51) 
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for a total performance index of 


(j) = t + k T (j ) T , k T > 0 user — selected (52) 

3. We generically deplore the use of penalty functions in trajectory optimization as being 
an unacceptably vague way of expressing performance goals and constraints. In this 
case, a penalty is introduced only to prevent a nonunique solution for the additional 
(u T )k and p T degrees of freedom, rather than to compete with the principal goal of 
minimizing r. 

With the introduction of two additional states and one additional control, the problem 
elevates from being completely trivial to being fairly simple. Since these additional variables 
are being manipulated in four different user routines, it behooves the user to take measures 
to avoid mistakenly picking off the wrong element of the xv or uv vectors in different routines. 
Define a FORTRAN Module: 

module bangMOD 
implicit none 
integer .parameter 
& locx2=2 
& locxtau=3 
& loctauint=4 
& locu=l 
& locutau=2 
& locptau=l 
real(8) .parameter 
& a=l 
& b=l 

& ctau=ld-6 
& ktau=ld-2 
end module bangMOD 

The relevant code fragments are shown below. Only the fragment for subroutine xdot 
shows the use of the module in the subroutine header, but similar “use” statements appear 
in cineq, phiobj, and psibc. 

xdot use bangMOD, only : locx2,locxtau,loctauint,locu,locutau,locptau 
implicit none 

integer , intent (in) :: nstate,kph,nx,nu,np 

real (8) , intent (in) :: xv(nx) ,uv(nu) ,pv(np) 

real (8) , intent ( (out) :: fv(nx) 

real(8) :: x2,xtau,tauint,u,utau,avetau 

x2=xv(locx2) 

xtau=xv(locxtau) 

tauint=xv(loctauint) 

u=uv(locu) 

utau=uv(locutau) 

avetau=pv(locptau) 

fv(l)=utau*x2 
fv(locx2)=utau*u 
fv(locxtau)=utau 
f v (loctauint ) = (utau-avetau) 


! enforce strong typing. Highly Recommended! 

:: & ! ‘ 'parameters 1 ’ are compile-time fixed constants 
& ! elements of xv 
& 

& 

& ! elements of uv 
& 

! element of p 

: : & 

& ! initial value of xl 
& ! initial value of x2 
& ! value of c_tau in u_tau constraint 
! penalty weight on penalty integral 
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cineq !use bangMOD , only : locu,locutau,ctau — info only; belongs in declarations 
u=uv(locu) 
utau=uv(locutau) 
c ( 1 ) =u+ 1 
c (2)=l-u 
c (3)=utau-ctau 

phiobj !use bangMOD, only : loctauint ,locxtau,ktau — info only; goes in declarations 
tauint=xbc (kf (l)+loctauint) 
tauf inal=xbc(kf (l)+locxtau) 
phi=tauf inal+ktau*tauint 

psibc !use bangMOD, only : locx2,loctau, loctauint, locptau — info only 
iebc=0 

psi(l)=xbc(kO(l)+l)-a ! plant boundary conditions 

psi(2)=xbc(k0(l)+locx2)-b 

psi(3)=xbc(kf (1)+1) 

psi(4)=xbc(kf (l)+locx2) 

psi(5)=xbc(k0(l)+locxtau) ! zero IC for utau 

psi(6)=xbc(k0(l)+loctauint) ! zero IC for penalty integral 

psi(7)=pv(locptau)-xbc(kf (l)+locxtau) ! place terminal value of utau in ptau 


4.2 

4 

3.8 

=s" 3.6 

3.4 

3.2 -. : : 

3 ; ; 

0 12 3 

time 



Figure 2. Two-State Bang-Bang Problem with Variable Time Step 

This problem was solved, as in the case of uniform time step (UTS), with 50 discretization 
intervals, i.e. nd = 50, and the penalty weight on variation in u T was set to k T = 1/100. 

The initial guess for this problem was taken from the solution of the baseline case in 
Section 2.1.1, using the following script to add the additional two states and one control to 
the input data: 


32 


S=importMADS ( 1 bangbanglpremads . dat ’ , 1 bangbanglout . dat ’ , 0) ; 
tau=p(length(S.p)) ; 

S .nuv=S .nuv+1 ; °/ 0 more controls 

S.nxv=S.nxv+2; °/ 0 more states 

S.npsi=7; °/ 0 more boundary conditions 

S.ncv=3; °/ 0 more trajectory constraints 

writepreMADS (S , ’ varsteppremads . dat ’ ) ; 
vout=adduxMADS (S . x , S . u , S . nxv , S . nuv , 0) 
vout= [vout ; tau] ; 

save varstepin.dat vout -ascii -double 

Figure 2 displays details of the solution. The plot on the left displays u T as a function 
of time. The plot on the right compares details of the control trajectory for the UTS case 
and the variable time step (VTS) case. The VTS does provide a clean “bang” and, in fact, 
returns a very slight improvement in terminal time - 3.4495s versus 3.4502s for the UTS 
case. It’s also interesting, but not surprising that the bang is initiated a little later for the 
VTS case. 

3.1.4 Eliminate the Bangs 

As was pointed out in Section 2.1, various functions of state and control variables can be 
formulated to take the place of “u” in f(x,u,p), and these can be manipulated to control 
their temporal behavior; for example, limiting bandwidth. In this subsection, an alternate 
approach to enforcing limits on control accelerations is described — in order to illustrate a 
useful trick available to the user with the ME discretization. 

The ME discretization can be adapted to provide a multistep delay buffer. Generally, 
this can be used to implement a sliding temporal window along the trajectory on which the 
problem formulation can impose conditions. For the current problem, we require three time 
steps for building up a numerical estimate for it, and implement the following differential 
equations in xdot without time scaling: 

ii\ = 2nd(— v\ + u ) 

V 2 = 2nd(— V 2 + 2vi — u ) 

where nd is the number of discretization intervals. These ODEs, when ME-discretized give, 
at the k th instant, 



(v 1 ) k =u k -i, (v 2 )k = u k -2 ( 54 ) 

Assuming a constant time step, u at the ( k — l) th instant is approximately 


^k— 1 


(u-2vi + v 2 )k ( 55 ) 

where nd is the number of discretization intervals, and the expression for u is 

( 56 ) 


Uk 


— ) (u - V\)k 


Obviously, this trick can easily be adapted to the case of variable time steps by appropriately 
modifying the numerical differentiation formulae. 
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With these control derivatives in hand, the bandwidth of the optimal control solution 
can be limited either through a slovenly penalty function approach, such as adding a term 
like 


f \u-2v i 
Jo 


v 2 ) 2 dr 


(57) 


to the cost function, or by imposing a constraint on some function of the acceleration. 

As an illustration of the latter approach, we consider the trade between |it| max - rather 


than the rms-like acceleration measure in (57) - and system performance. This trade can 
be explored either by fixing r and minimizing |u| max or vice versa. Pretend that our double- 
integrator is actually a physical system, and that the value of |it| max plays an important role 
in its cost of manufacture. Pretend, further, that there is a range of potentially acceptable 
r and that we want to find the “knee in the curve” in the relationship between |ii| max and 
r. Since we know what r we can tolerate, constrain r and minimize |u| max by solving the 
problem of minimizing 


subject to 


0 = Pddmax (phiobj) 


Pddmax 


< ( ! y) 2 {u - 2v\ + V 2 )k < Pddi) 


Pddmax _ 


> 0 


(cineq) 1 
(psibc) J 


(58) 


(59) 


0 = lf) T = T - Tgiven (psibc) (60) 

and, implemented in xdot and psibc, 


~ -7 — --j- * - — 

Xi = 

j 7 

tx 2 , 

a?i(0) = a, Xi(l) = 0 

x 2 = 

TU , 

x 2 (0) = b , 2 : 2 ( 1 ) = 0 

Vi = 

2(— UL + u), 

boundary values free 

v 2 = 

2( v 2 + 2v\ - u), 

boundary values free 



Note that the acceleration constraints in ( 59 ) are indexed on the k th instant rather than the 
( k + l) th . This is necessary in order to have u in its proper place in the u, Vi, v 2 sequence 
of points. 

Finally, why specify r via the constraint (60 1 rather than by merely using the constant 
7 g iven as the duration parameter? The lagrange multiplier associated with ip T iven in ( 60 1 is 
available from SNOPT (and hence, MADS) upon successfully converging a solution, anawill 
supply useful information in computing the trade between T g i ve n and | it | max- In particular, 
as is well-known, express a hypothetical cosntrained minimization problem as 


x* = arg min f ,c r i_n C(x) 

c = m-Kc (*)-<%.„ 


(62) 


where S is a hypothetical small variation in the constraint setting. By different ating C by 6 
it’s easy to see that 


C* s = X* (63) 

Thus, for our trade, the sensitivity of minimum peak control acceleration with respect to 
fixed terminal time constraint setting is A* given . 

Figure [3] displays solutions to this problem for several values of T g i V en corresponding to 
0.5%, 1%, 2%, and 5% degradation in r from the perfect “bang-bang” value of r = 3.4495s. 
The plot on the right displays the resulting control histories for each r g i ven . The plot on the 
left displays a hermite spline that uses the pddmax solution data and the associated lagrange 
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Figure 3. Two-State Bang-Bang Problem with Variable Time Step 


multipliers for (60 1 to provide an estimate of peak \u\ away from the MADS solutions. The 
hermite spline used here is a cubic polynomial y{s) defined on the interval 0 < s < x that 
satisfies the boundary conditions 


y(o) = m 

y(x) = y{x) 


y'{ 0) = y\x) 
y '(x) = y'{ x) 


(64) 


where the barred quantities are data. Thus, the lagrange multpliers provide “free” data that 
permit a cubic fit between pairs of solutions points, rather than the quartuples of solution 
points that are needed with cubic splines that operate only on point values. In this case, 
the y are the constrained system performance, i.e. the peak acceleration magnitudes, and 


the y' are the lagrange multipliers for the constraints ( 60 ) . Table 3.1.4 displays the values 
of Tgiven, Pddmax, and the lagrange multiplier A 


Table 1. Data From MADS Solutions for Various Values of 


Tgiven 

Pddmax 

given 

3.4668 

15.598 

— 1.071(10 3 ) 

3.4840 

7.8212 

— 2.021(10 2 ) 

3.5185 

3.8812 

-s.m^io 1 ) 

3.6220 

1.5223 

-9.8350 


3.1.5 Problem Summary 

This example has provided several things useful to the aspiring MADS user. They are listed 
below, associated with the subsection in which they appear: 
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1.1 1. Code fragments shown for casting a simple problem in MADS format. In particu- 

lar, treatment of boundary conditions and cost is demonstrated, and an informal 
discussion of measures to be taken in using an autodifferentiation code for prepar- 
ing the model for MADS is given. 

2. Description of obtaining an initial guess, including a warning that the user needs 
to be wary of allowing free terminal time problems to freely vary terminal time. 

3. The impact of fixed discretization stepsize on the fidelity with which discontinu- 
ous phenomena are preserved from the continuous time problem is demonstrated. 

1.2 1. Brief description of breaking the original problem into two subarcs in order to 

recover the “bang-bang” control behavior is provided. Multiple-subarc problems 
will be dealt with in more detail in the next example 

1.3 1 . Technique for allowing variable discretization time step in the context of a single 

arc. 

2. Demonstrated use of psibc to make state boundary values available to the system 
differential equations. 

1.4 1 . Demonstration of using the structure of the midpoint euler discretization (kode = 

0) to model time derivatives of the optimal control solution. This permits direct 
manipulation of the temporal characteristics of the control without resorting to 
ineffective artifices such as placing filters into the plant dynamics to bandlimit 
the control. 

2. Code for changing the structure of a single-phase MADS run’s output data to 
provide an input guess for a MADS problem with that different structure. A 
subsequent problem will demonstrate breaking a run into multiple phases and 
changing the discretization density. 

3. Demonstration of minimizing a variable’s maximum value. The same approach 
can be used for constraining the same. 

4. Demonstration and discussion of using lagrange multipliers that are output by 
SNOPT to provide better continuous realization of the variation of constrained 
system performance with constraint settings. This is of particular importance for 
getting the best possible performnace surveys when it is not feasible to perform 
a large number of MADS runs. 

3.2 Goddard Problem 

The Goddard problem [8] is a nonlinear optimal control problem in which a sounding rocket 
travels vertically to maximize its peak altitude. For the version of the problem considered 
here, the rocket flies through an atmosphere whose density decays expontially with altitude, 
the trajectory is terminated at its peak altitude, and an inequality constraint is imposed 
on the maximum dynamic pressure to which the rocket is exposed. The dynamics, taken 
from [8], have nondimensionalized states r - radius from Earth’s center, V - speed, and to 
- mass. The state equations are 


r = V 

(65) 

T-D 1 

(66) 

V = 

v 9 

m r z 

to = — T/c, c= 1/2 

(67) 
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where T is the control, thrust, satisfying 


0 < T < 3.5 

and D = Knq{r, V ) is drag, with Kp = 620, and 


q = -y exp ((3(1 — r )) , (3 = 500 
is dynamic pressure. The dynamic pressure constraint is simply 


( 68 ) 


(69) 


<7max > q(r, V ) (70) 

Note that this is a state inequality constraint; that is, the control, T, does not appear in 
( 70 1 . The boundary conditions for the trajectory are 


r(0) = 1 ^(0) = 0 m(0) = l\ . . 

V(t f ) = 0 m(t f ) = 0.6 / U j 

The problem is solved by determining the thrust history that maximizes the terminal altitude 
when the rocket runs out of fuel; that is, 


(72) 

Aside from the nonlinearity of the plant and the presence of a state inequality constraint, 
the problem introduces us to an additional complication in the form of a “singular arc,” 
described in Chapter 8 of [5] . Loosely speaking, a singular arc is a finite-length subinterval of 
a continuous-time optimal trajectory during which the trajectory cost function’s sensitivity 
to control vanishes to first order. In other words, to first order, the optimal control problem 
simply doesn’t care what the control does during a singular arc. There is, indeed, an optimal 
solution for the control, but its computation typically requires recourse to the calculus 
of variations (COV) to obtain higher-order necessary conditions for optimality, which are 
solved as a system of equations. The MADS user, on the other hand, uses first derivative 
information to directly minimize the cost function via NLP. In this case, it will be seen that 
the singular arc exhibits itself by a tendency toward ugly, jittery, behavior in the control 
during the singular arc. 

This example will carry the user through solving the Goddard problem several different 
ways, and at several different levels of complexity. Although the basic problem is nearly 
trivial to solve, the user does have choices available in how to organize plant and inequality 
constraints, whether to suppress numerical artifacts in the control during singular arcs, or 
whether to optimze the distribution of discretization intervals to improve accuracy. 

Before beginning the Goddard solutions, we propose guidelines for coding them - and 
the user’s own problems. There are three main considerations in setting up problems in 
MADS. 

• Separate Plant Code: 

There are typically more states and controls in the optimization problem than in the 
plant model. This was seen in the min-time linear problem of Subsection 3.1, and 
will be particularly seen in the Goddard problem. Because of this, the plant-specific 
modelling - particularly the plant dynamics - should be coded separately, to facilitate 
reliable re-use. 

• Named Scalars from Vectors: 

Nonlinear models are characterized by scalar computations. Therefore, readability of 
code will be enhanced by separating these variables out from MADS state, control, 
and parameter vectors. For example, if xv = [r, V, m] is the state vector, then the 
plant dynamics will be more readable using scalars than vector references. 
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r=xv ( 1 ) 
V=xv(2) 
m=xv(3) 
T=uv(l) 


D=620*exp (500* (1-r) ) *V**2/2 
fv(l)=r 

fv(2)=(T-D)/m-l/r**2 

fv(3)=2*T 


This is still seriously flawed, though. Consistent naming is needed throughout the 
problem code, and hardwiring this variable name indexing scheme in each MADS user 
routine would be horribly fragile; moreover, hardwiring plant dynamical parameters 
reduces code clarity and makes it difficult to update the model. 

• Use a MODULE to Set Indices and Parameters: 

MADS uses four user-supplied routines to instantiate any trajectory problem, and 
plant information is used by each of them, so that having a single location in the 
user code for defining shared indices and parameters is essential. The FORTRAN 
MODULE provides an easy means to have such sharing. For our example, 


module rocketMOD 
implicit none ! enforce 


integer , parameter : : & 
& locr=l, & 
& locV=2, & 
& Locm=3, & 
& locT=l, & 
& nxp=3 , & 
& nup=l, & 
& kxp=l, & 
& kup=l 


strong typing (highly recommended!) 

‘ ‘parameters ’ ’ are compile-time fixed constants 

radius 

speed 

mass 

thrust (control) 
plant state dimension 
plant control dimension 
index beginning plant state 
index beginning plant control 


real (8) , parameter : : 
& KD=620 , 

& beta=500, 

& ISP=0 . 5D0 , 

& mEmpty=0 . 6D0 , 

& Tmin=0 , 

& Tmax=3 . 5D0 

end module rocketMOD 


drag coefficient 

atmospheric density lapse rate 

specific impulse 

rocket empty mass 

min thrust 

max thrust 


This permits us to rewrite the plant dynamics as 

subroutine rocket (xp, up, fp) 

! note that ‘'only’’ keyword prevents '‘rocket’’ 
! from seeing nxp, nup, mEmpty, Tmin, Tmax 
use rocketMOD , only : locr,locV,locm,locT,KD, beta, ISP 
implicit none 

real (8) , intent (in) :: xp(3),up(l) ! state and control 
real (8) , intent (out) :: fp(3) ! state rate 

real(8) :: r,V,m,T,D 
r=xp(locr) 
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V=xp(locV) 

m=xp(locm) 

T=up(locT) 

D=KD*exp (beta* (1-r) ) *V**2/2 
fp(locr)=V 

fp(locV)=(T-D)/m-l/r**2 

fp(locm)=T/ISP 

end subroutine rocket 

This approach is used throughout the examples, described below. Note that the source 
code for each example is located in Appendix GoddardAppx. 

The organization of the remainder of this Section is as follows: In Subection 3.2.1, an 
initial guess for the problem is generated. Next, (Subsection 3.2.2,) the problem is solved 
directly, with and without a dynamic pressure constraint. Subsection 3.2.3 introduces an 
adhoc but effective penalty function which can be used to correct singlarity-induced freaks 
in the direct solution. 


3.2.1 Obtaining an Initial Guess 


Because the problem is substantially nonlinear, it will not do to simply choose random 
numbers as the initial guess for the Goddard problem. Yet, at the same time, we would 
prefer to keep the initial guess workload as low as possible. Experience indicates that 
boundary conditions are frequently the most difficult constraints to satisfy in generating a 
feasible trajectory in MADS. If an optimal MADS trajectory can be obtained for boundary 
conditions that have been “relaxed” in some way by starting from a random initial guess, 
perhaps such a solution will be an adequate initial guess for the problem with boundary 
conditions enforced. 

It is reasonable to initially try solving a trajectory problem with the rocket dynamics as 
per ( 65||67 I and cost function (72 1, but with the boundary conditions relaxed. This is done 
by moving them over to cost as penalty terms: 


</> = -r(t f ) + I< * (r(0) - l) 2 + V 2 {0) + V 2 (t f ) + (m(0) - l) 2 + (m(f/) - 0.6) 2 (73) 

where K > 0. 

The highlights of the code are summarized below. Since this is a problem with free ter- 
minal time, the trajectory duration is introduced as an element of the MADS pv vector, and 
used to scale the RHS of the plant differential equations. The code fragment in subroutine 
xdot is 


call rocket (xv(kxp) ,uv(kup) ,fv(kxp) ) 

tau=pv (loctau) 

fv=tau*fv 


The code implementing (68) in subroutine cineq is 


if (iecinf lag .EQ . l)then 

iecinv=l ! all cineq constraints are inequalities 
return 
endif 

T=uv(locT) 

cv(l)=Tmax-T 

cv(2)=T-Tmin 
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and the implementation of the cost function (73) in subroutine phiobj is 


rO=xbc(kO(l)+locr) 
rf=xbc(kf (l)+locr) 

VO=xbc(kO(l)+locV) 

¥f=xbc(kf (l)+locV) 
mO=xbc(kO(l)+locm) 
mf=xbc(kf (l)+locm) 

phi=-rf+K* ( (rO-1) **2+¥0**2+¥f **2+(m0-l) **2+(mf-mEmpty) **2 ) 

For this problem, psibc implements only a constraint ensuring that optimal duration r* is 
not zero or negative: 


ip = T- T min > 0 (74) 

The parameters r m ; n and mEmpty pertain to the problem definition, rather than the plant 
description. As such, they are placed in a separate module file: 

subroutine GGUESSMOD 
implicit none 
real (8) : : & 

& taumin, & 

& K 

end GGUESSMOD 

An initial guess for this problem is generated from random numbers in makeGGUESS .m, 
given in the Appendix, is essentially 

randmag=0 . 01 
P . nxv=3 ; 

P.nuv=l; 

P . nav=2 ; 

P.ndv=200; 

P.np=l ; 

P.nph=l; 

P.kodev=0; 

P.nbc=l; 

vin=randguessMADS (P . nxv , P . nuv , P . ndv , P . np , randmag) +1 ; 
save GGUESS_vin.dat vin -ascii -double 
name= ’ GGUESS_premads . dat ’ ; 
writepremadsMADS (P , name) 

The files generated by this code correspond to the main routine code 

program GGUESS 

use GGUESSMOD , only : taumin, K 

taumin=lD-4 

K=100 

Finput=13 

open ( 13 , f ile= 1 GGUESS_vin . dat ’ , status= 1 old ’ ) 

Fpremads=14 

open ( 14, f ile=’ GGUESS_premads . dat 1 , status=’ old’ ) 

Foutput=15 

open ( 15, f ile=’GGUESS_vout .dat ’ ,status=’ unknown 1 ) 
call batchMADS(Finput ,Fpremads,Foutput , info) 
end program GGUESS 
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Finally, the solution is organized for plotting in Matlab by executing 

S=importMADS ( ’ GGUESS_premadst . dat 1 , ’ GGUESS_vout . dat 1 , 0) 

where S contains the solution data in a form suitable for graphics. The details of importMADS, 
recall, are given in Section 1.3.2. 
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Figure 4. Initial Guess for Goddard Problem 

Figure [4] displays the solution to this problem. Note that the altitude boundary condi- 
tions are seriously violated, and that the thrust profile is substantially different from that 
in Figure [5] lacking the intermediate thrust arc that appears in the optimal solution. Ef- 
fectively, the solution has moved the starting point for the trajectory to a higher altitude, 
where the atmosphere is thinner, reducing drag, so that the “bang-bang” thrust solution 
produces optimal altitude gain. 

This section has provided the following: 

• It has provided and illustrated advice on organizing a MADS problem. The key points 
are - 

1. Separate plant-related code from the rest of the MADS problem code, because 
the MADS formulation may introduce additional states, control, and parameters. 

2. Provide a consistent, globally available, index to identify each individual element 
of state, control, and parameter vectors. 

3. Place these in MODULES, separating the MODULE or MODULES dedicated to the plant 
dynamics from those that will carry values related only to the particular problem 
formulation. 

• It has discussed a philosphy for starting MADS solutions from initial guesses that 
consist only of random numbers. This was demonstrated by posing a problem with 
boundary conditions eliminated and replaced by a penalty function. While we do not 
claim that it will always work, it’s a good place to start. 
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3.2.2 Simple Solution with Dynamic Pressure Constraint 


This subsection provides the direct, unconstrained solution for the Goddard problem, and 
demonstrates two approaches for imposing constraints that involve states in cineq. The 
unconstrained problem is posed by retaining xdot and cineq from Subsection 3.2.1, and 
moving the boundary conditions from the penalty function (73 1 to psibc from phiobj. In 
psibc, 


if (iebcflag.EQ. l)then 

iebcvec=0 ! The boundary conditions are equalities 

iebcvec(6)=l ! The constraint on tau versus taumin is inequality 

return 
endif 

psi(l)=xbc(kO(l)+locr)-l 

psi(2)=xbc(kO(l)+locV) 

psi(3)=xbc(k0(l)+locm)-l 

psi(4)=xbc(kf (l)+locV) ! Not theoretically necessary, but sharpens the problem 
psi (5) =xbc (kf (1) +locm) -mEmpty 
psi (6) =pv(loctau) -taumin 

and, in phiobj , 

phi=-xbc(kf (l)+locr) 

Note that for the very simple and unrepetitive expressions in psibc and phiobj, here, 
we’ve not bothered to break out individually named scalar variables. The code, as written, 
is perfectly readable. 

Assuming that the initial guess has been stored in GGUESS.mat as a importMADS struc- 
ture “S,” the script for setting up the inputs for the MADS run will be 

load GGUESS "/, Assume this .mat file contains the structure S from using 
°L importMADS on GGUESS_vout . dat and GGUESS_premads . dat 

randmag=0 . 0 ; 

P=S; "/, Mostly, this problem is structured the same as GGUESS. 

P.nbc=6; "/« This problem has a different number of boundary conditions 

vin=adduxMADS(S.x-[l}-,S.u{l},P.nxv,P.nuv,randmag) ; °/ 0 This reorganizes S.x and 

% S.u back into a vector. . . 
vin=[vin;S.p] ; % and the duration parameter is tacked onto the end 

save Glunc_vin.dat vin -ascii -double 
name= ’ Glunc_premads . dat 1 ; 
writepremads (P .name) 

We now solve the problem, operate on the output and “premads” files with importMADS, 
and save the resulting structure in Glunc .mat. Figure[5]displays the resulting altitude/speed 
and thrust plots. The most noticeable feature of the displayed solution is the noisy appear- 
ance of the thrust history. Why is this? The optimal trajectory for this case is “singular”, 
and can be explained by informal appeal to variational optimal control theory. 

In variational optimal control, the Minimum Principle [6], [7], states that the optimal 
trajectory minimizes the problem’s hamiltonian function, TL, where, for x = /(a;, u), 

H = X T f(x,u), X = — /JA (75) 
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Figure 5. Goddard Solution Without Dynamic Pressure Constraint 


and A is a vector roughly analogous to the instantaneous value of trajectory of lagrange 
multipliers. Because H is to be minimized by thrust T(f), its partial derivative trajectory 
TLt provides a useful diagnostic. When Tir ^ 0, optimality requires 


1 

f T min 

for 

Ht > 0 


T* = l 

Ti nax 

for 

Ht< 0 

(76) 

1 

[_ some intermediate value 

for 

Ht = 0 



The “singularity” in this problem arises from the fact that, since T enters the dynamics 
linearly in ( 66 1 , T is absent from 'Hti so that it cannot be used to determine optimal T*. 
Additionally, TLtt is identically zero (hence singularity.) In practical terms, up to second 
order, the optimal trajectory simply “doesn’t care” about the particular value of T when 
TL = 0. This, in turn, means that for our direct minimization solution process, the absence of 
performance impact due to thrust deprives the solution iterations information necessary to 
settle on a locally unique thrust trajctory, even though those iterations satisfy the solution 
algorithm’s convergence criteria. 


Figure[6]displays the optimal trajectory of Ht for this problem, and the sequence of T max , 
followed by some intermediate value, followed by T m j n is clear. This Figure, incidentally, 
was generated by using MADS to emulate a variational solution to the Goddard problem, 
posing a discretization of the variational necessary conditions for optimal control as a penalty 
function to be minimized by meeting the boundary conditions and constraints. 


This singular behavior is not a mere curiosity. It most frequently occurs in problems in 
which the dynamics are linear in one or more control variables - a category that includes 
aerospace systems that involve propulsion. An extremely coarse cure for this issue would 
be to introduce a nonlinearity in T into the problem dynamics, such as an integral thrust 
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Figure 6. Hamiltonian Thrust Partial Derivative for Goddard Problem 


penalty, 


^penalty = ~r(r) + f T 2 dt (77) 

Jo 

Such a penalty would cure the thrust jitters, since the additional state for the penalty 
integral is nonlinear in T. The cure comes at the cost, however, of moving the performance 
goal away from maximizing the terminal altitude, and toward minimizing thrust usage. We 
are no longer solving the original problem. Short of adopting a full variational solution, 
it is best to introduce the needed nonlinearity in a way that only neglibly affects optimal 
performance, particularly during the nonsingular problem phases. 

Let’s introduce a dynamic pressure constraint: 


9max (?(D ^0 ^ 0 (1 

Q = it - r )) J 


(78) 


This is the first instance, in this tutorial, of a state inequality constraint. Since, in MADS, 
control is held constant across each discretization interval, expressing a control-only in- 
equality is straightforward. What of the case where states are involved in the constraint 
expression, as in (78 1? States appear at different instants in cineq, and in the discretization 
logic that calls xdot. In cineq the state is available at the beginning (xj) and end (xjpl) of 
each discretization interval. Depending on the value of kode, xdot may be called with the 
midpoint average (xj + xjpl)/2, for kode = 0, or with several extrapolated values, when 
using the Runge Kutta (RK) discretizations, kode = 1,2. 

In the case. of the RK discretizations, there is no choice but to compute inequality con- 
straint quantities in cineq using state values from xj or xjpl. In the mindpoint Euler 
case, however, the user has the option of computing a constraint quantity, say “y,” in xdot, 
assigning a state, say xv(locy), to it and using an expression like 
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fv(locy) = 2 * nk * (y — xv(locy)) 


to propagate it, so that the current value of y is available to cineq at xjpl(locy). Recall 
that we introduced this trick in the Minimum-Time Double Integrator problem in Subsection 


2.1.4. 


Why would one chose to compute y this way? The most direct motivation for doing 
this would be when computation of y is tightly integrated with the software called in xdot 
for computing the RHS of the plant ODEs. Extracting y from the existing plant dynamics 
computations might be preferable, from a software point of view, to redundantly coding the 
logic for computing y. Furthermore, if the computation of y is not only tightly integrated 
with the plant dynamics, but also expensive, the user has further incentive to compute y in 
xdot. 

If the user chooses to set the problem up with cineq- related quantities computed in xdot, 
it must be remembered that they’re being computed at the midpoint of the integration 
interval, i.e. at (xj+xjpl)/2 in the notation of the cineq argument list. The practical 
implications of this are explored in the remainder of this example. 

The dynamic pressure constraint (78 1 is implemented in subroutine calcqbar: 


subroutine calcqbar (xv,qbar) 

use rocketMOD.only : c,kD,beta,locr,locV,nxp 

implicit none 

real (8) , intent (in) :: xv(nxp) 
real (8) , intent (out) :: qbar 
real (8) :: dexp 
real (8) .parameter :: 0NE=1.D0 


real (8) : : r , V 
r=xv(locr) 

V=xv(locV) 

qbar=dexp (beta* (ONE-r) ) *V**2/2 
end subroutine calcqbar 

For the formulation in which qbar is computed in xdot with kodev=0, the relevant lines of 
xdot and cineq are 

real(8) :: tau.qbar 

call rocket (xv(kxp) ,uv(kup) ,fv(kxp)) 

tau=pv (loctau) 

f v (kMain) =tau*f v (kMain) 

call calcqbar (xv(kxp) , qbar) 

f v (locqbarm) =2*nk* (qbar-xv (locqbarm) ) 

and 

if (iecf lg. EQ . l)then 
iecin=l 
return 
endif 

cv(l)=Tmax-uv(locT) 
cv(2)=uv(locT)-Tmin 
cv(3)=qbarmax-xjpl (locqbarm) 

respectively. With qbar computed in cineq, the code in xdot is 
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call rocket (xv(kxp) ,uv(kup) ,fv(kxp)) 

tau=pv (loctau) 

fv=tau*fv 

and 


if (iecf lg. EQ . l)then 
iecin=l 
return 
endif 

c v ( 1 ) =Tmax-uv ( lo cT) 

cv(2)=uv(locT)-Tmin 

call calcqbar (xj (kxp) ,qbar) 

cv(3)=qbarmax-qbar 

Note the differences in xdot. For the kodev=0 case, (the former,) the time scaling is 
restricted to fv(kMain), rather than to all of fv. The index vector is set in GlM0D.f90, 
given below: 


module G1M0D 

use rocketMOD , only : nxp 
implicit none 


integer .parameter 
k locqbarm=nxp+l 


state for carrying qbar to cineq 


integer .parameter 
k loctau=l 


time-scaling parameter in pv 


real (8) .parameter 
k mfinal=0.6D0 


empty mass 


integer : : kmod 

integer .parameter : : k ! indices identifying states to be time-scaled. The 

! lag state carrying qbar is *not* time-scaled. 
k kMain ( nxp ) = (/ (kmod, kmo d= 1 , nxp ) / ) 


real (8) : : k 

k taumin, k ! 

k qbarmax ! 

end module G1M0D\ 

The index vector kMain is initialized to kMain= [1 , 2 , 3] , above, and the state variable for 
passing qbar is given the index locqbarm=nxp+l, appending it the plant state vector as a 
fourth state. 

Both of the above versions of the qbar-constrained problem were started using the uncon- 
strained solution as an initial guess. For the former case, the additional state xv(locqbarm) 
was added to the input guess vector using the script 

S=importMADS ( 1 Glunc_premads . dat ; , 1 Glunc_vout . dat 1 , 0) ; 
randmag=0 . 0 ; 

P.nxv=4; 7. note additional state 

P . nuv=S . nuv ; 
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qbar (X 1 0 4 ) qbar (X 1 0~ 4 ) qbar (X 1 0 


P.nav=3; °/ 0 additional inequality constraint 

P . ndv=S . ndv ; 

P . np=S . np ; 

P . nph=S . nph ; 

P . kodev=S . kodev ; 

P.nbc=S.nbc; 

vin=adduxMADS(S.x{l},S.u{l} J P.nxv,P.nuv,randmag) ; °/ 0 This will add a state 
vin=[vin;S.p] ; 

save Gl_vin.dat vin -ascii -double 
name= ’ Gl_premads . dat ’ ; 
writepremads (P ,name) 

The script converts the “Glunc” output from a column vector to a importMADS structure, 
and then uses “adduxMADS” to create a new vector with the fourth state appended. 



0 0.1 0.2 0 0.1 0.2 
time time 




Figure 7. Thrust Solution Behavior for Several Values of qbarmax 

Figure [7] compares the behavior of the optimized thrust for several values of qbarmax 
(unconstrained maximum q is roughly 1.2(10 -3 ), and for q computed in xdot versus q com- 
puted in cineq. Scanning from the left, the first column displays the q histories (taken from 
the cases where q was computed in cineq). The middle column displays the corresponding 
optimized thrust histories where q was computed in xdot, and the third column shows the 
corresponding thrust histories with q computed in cineq. All of these runs successfully 
converged. 
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Obviously, Figure [7] demonstrates that computing the state constraint quantity in xdot 
leaves the user vulnerable to misbehavior in the control solution. The reason for this is that 
satisfying the constraint at the midpoint of each discretization interval does not guarantee 
that the constraint rate is zeroed. At this point, Murphy’s Law takes charge. This does not 
necessarily mean that it never makes sense to compute the constraint quantity in xdot, but 
it certainly indicates that additional measures need to be taken in this case, before the user 
can be confident of acceptable results. 

There is more to be seen in this Figure. Observe how the q profile for qbarmax = 7(10 -4 ) 
falls off at the end of the active constraint arc, signalling a probable singular arc. This 
tentative diagnosis is supported by the choppy behavior of the thrust solution in column 
three. Alternatively, for qbarmax = 5(10 4 ), the active constraint arc completely eliminates 
the singular arc, giving a very crisp T max — constrained — T m ; n thrust profile. For qbarmax = 
7(10 -4 ), however, close examination of the thrust profile shows a little rounding of the thrust 
history near the end of the active constraint arc. Is this a very short singular arc? The 
only way to tell for sure is by solving the corrsponding variational optimal control problem, 
which can be quite intricate, per Chapter 8 of [5]. 

The user may very likely not care about the forensic details that are opened up by a full 
variational analysis, but would just like to tame bad control behavior on singular arcs. One 
way of doing this is assign a penalty function designed to suppress jitter without interfering 
with large control movements. This approach is described in the sequel. Before proceeding, 
quickly review what has been introduced in this Subsection: 

• Two different approaches for imposing a state inequality constraint were described 
and demonstrated, and pros and cons of each were discussed. 

• One approach (computing constraint quantities in xdot) requires an additional state. 
Logic for adding states to an input guess was demonstrated. 

• Logic for restricting time scaling of the state derivative to a subset of the states was 
shown. 

3.2.3 A Penalty Function to Smooth Out Singular Jitter 

The easiest, and least elegant, way to suppress singularity-induced control jitter is to pe- 
nalize it. Looking at Figure [7J the reader will agree that control jitter is characterized by 
persistent, large, acceleration; so, it would make sense to penalize control acceleration. At 
the same time, however, it would be best if the penalty did not suppress or distort the rapid 
“bang-bang” control shifts associated with leaving T max and moving to T m ; n ; they are, after 
all, optimal. A compromise penalty would penalize control acceleration more heavily when 
control rates are low, with the expectation that superfluous acceleration (jitter) would dis- 
appear, leaving necessary accleration (step changes) relatively intact. Such a penalty would 
take the form 


0 pen 


-dt 


(79) 


/ 0 c pen 


where it should be noted that we recommend that the penalty state be integrated from 0 to 
1, i.e. , without time scaling. This could be implemented directly by redefining the control 
variable to be thrust acceleration, i.e., u = T and appropriately introducing additional 
states to realize (79) and T. MADS approximates (79) in subroutine ddpenMADS, which 
differences across time steps to model acceleration and rates: 
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(80) 


48 


where the (•)& subscripts denote the time index in MADS. Implementing the time differ- 
encing in ddpenMADS requires three states, and the penalty integral introduces a fourth 
state. 

This penalty is applied to the problem of Subsection 3.2.2 by adding the states needed 
for ddpenMADS and jTddpem and including the penalty in the performance index: 


(j) — t(7") T AperuiTddpen(l) 

where K pea > 0 is user-specified. The variables are organized in 


module SL01M0D 

use rocketMOD , only : nxp 

implicit none 

integer .parameter : : & 

& locqbarm=nxp+l , & 

& locddpv=locqbarm+l , & 

& locddpint=locddpv+3 


carry qbar to cineq (4) 
3-element state for ddpen (5) 
ddpen penalty integral (8) 


integer .parameter : : & 

& loctau=l ! time-scaling parameter in pv 


(81) 


integer : : kmod 

integer .parameter : : & ! indices identifying states to be time-scaled. The 

! ddpen states not time-scaled. 

& kMain (nxp) =(/ (kmod, kmod=l ,nxp) /) 

real (8) : : & 

& taumin, & 

& qbarmax , & 

& epsvdd, & 

& Kddp 

end module SLD1M0D 

and the logic in xdot becomes 

call rocket (xv(kxp) ,uv(kup) ,fv(kxp) ) 
call calcqbar(xv(kxp) ,qbar) 
f v (locqbarm) =2*nk* (qbar-xv (locqbarm) ) 

! Note that 4th argument of ddpenMADS is ‘‘3’’ That is the number of 
! states that the user needs to make available to ddpenMADS, and the 
! subroutine checks to see that the user has at least declared that he has 
! provided the right number of states, 
call ddpenMADS (nk.uv(kxp) .xv(locddpv) ,3, epsvdd, fv(locddpv) , fv(locddpint) ) 

tau=pv (loctau) 
f v (kMain) =tau*f v (kMain) 

there is no change in cineq, but the integral state Uddpen does require a zero initial condition, 
so that the logic becomes 

psi(l)=xbc(kO(l)+locr)-l 
psi (2)=xbc(kO(l)+locV) 


denominator bias term in ddpen 
weighting term for ddpen penalty 
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Percent Altitude Degradation 


! zero I.C. for penalty integral 


psi(3)=xbc(k0(l)+locm)-l 
psi (4) =xbc (kO ( 1 ) +locddpint ) 
psi(5)=xbc(kf (l)+locV) 
psi (6)=xbc (kf (l)+locm) -mf inal 
psi (7)=pv(loctau) -taumin 

The cost function is implemented in phiobj as 

rf=xbc(kf (l)+locr) 

phi=-rf + Kddp*xbc(kf (l)+locddpint) 

Unconstrained solutions for K pen = {10 -3 , 10 -2 , 10 — 1 } and e pen = 10 -2 were obtained using 
the solution from Figure [5] as an initial guess, and the results are displayed in Figure [HJ The 
left panel dispays the falloff in altitude performance as percentages for the several values of 
K pen . The right panel displays the T time histories for the K pe n = 1CU 3 and 1CU 1 cases. 
If one enlarges the thrust profile for A' pen = 10 -3 , small jitters are visible, but there is a 
substantial improvement from the behavior shown in Figure [5] The profile for A' pen = 10“ 1 
is smooth, but shows very little similarity to the optimal solution. Nonetheless, referring 
to the Figure’s left panel, there is only 0.03% degradation in altitude performance. This is 
a consequence of the presence of the singular arc - recall that the optimal performance is 
almost entirely insensitive to the particular thrust profile over a significant portion of the 
trajectory. 
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Figure 8. Variation of Altitude Performance and Thrust Behavior with A'pen 

Solutions were next generated using the jitter penalty, for the same values of qbarmax 
as those displayed in Figure [ 7 ] Figure [9] displays the resulting thrust profiles. In the left 
column of the Figure, all runs used the solution for A' pen = 10 -3 from Figure [5] as the 
initial guess, and AT pen and e pen were selected as displayed on each panel. The Figure’s right 
column display thrust for the case where the solutions were “walked in,” i.e. , the solution for 
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qbarmax = 9(10 -4 ) is started from the unconstrained case, that for qbarmax = 7(10 -4 ) is 
started from the solution for qbarmax = 9(10 -4 ), and so on. We see that the latter approach 
provides cleaner-looking solutions for less intrusive /\ pen , e pen settings. 
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Figure 9. Comparison of ^-constrained Thrust Profiles with Jitter Penalty 


We most particularly note, however, that all of these ^-constrained runs required heavier 
jitter penalties than the unconstrained case from Figure[8j Recall that, in the unconstrained 
case, the singular arc jitter exists because the optimization is largely oblivious to the value of 
T; therefore, a very small penalty suffices to correct the jitter. In the actively g-constrained 
cases, the optimization is taking advantage of the midpoint Euler discretization , settling 
on a choppy T solution to maximize terminal altitude. A heavier penalty is required to 
compete with this, and the overall T solution suffers additional distortion. 

Before proceeding further, let’s review where we are in solving the Goddard problem. 
We saw in Figure [8] that, using ddpenMADS, we could obtain a fairly clean, fairly undistorted 
unconstrained solution. We also saw that, even if we did penalize singularity-induced jitter 
so heavily that the thrust profile changed significantly from the true optimal solution, it 
wouldn’t matter have much impact on performance. In imposing an inequality constraint 
on g, we saw, from Figure [ 7 ] that we had our best results computing the constraint in cineq 
rather than in xdot. The trajectory for qbarmax = 9(10~ 4 ), though, is marred by an untidy 
singular arc that follows the active constraint arc. 

What can be done about this latter case? The simplest thing would be to apply a 
ddpenMADS penalty to the problem, while using cineq to compute q. The result, for K pen = 
10~ 4 , er, fin = 10 -2 is shown in Figure [lOl Like the lightly penalized unconstrained case 


solution, probably good enough for any practical 


= 10 -2 is shown in Figure 
in Figure |8j it provides a “pretty good” 
application. 

What - in all of these singular cases - if we want cleaner results? Consider, again, the 
qbarmax = 9(10 -4 ) case from Figure [ 7 ] It is easy to see that the optimal trajectory goes 
through four distinct phases: max thrust - max q - singular arc - min thrust. Conceptually, 
we could partition the trajectory and apply each phase’s appropriate constraints, one at a 
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Figure 10. 


Comparison of q-constrained Thrust Profiles with Jitter Penalty 


time: Require: 


Phase 1 . . . 
Phase 2 . . . 

Phase 3 . . . 

Phase 4 . . . 


T 

max 


= T, 


T 

max 


T 

max 


> T > T 

_ J _ 1 mi: 

> T > T ■ 

_ ^ ^ mir 


T>T r 


— min i 


qbarmax > q 
qbarmax = q 

qbarmax > q, 

qbarmax > q 


ddpenMADS \ ^P en A? , 
e D en = 10 


The result is shown in the upper plots in Figure [lT] The lower pair of plots in the Figure 
are the data from Figure [TO] There are clearly some differences between the two problem 
formulations, but we will defer their discussion until after showing code fragments for setting 
up the four-phase problem. 

Multi-phase problems have several key differences from single-phase problems, that must 
be kept in mind: 


1. Each phase will have its own duration. For this problem, the relevant lines of xdot 
are 


call rocket (xv(kxp) ,uv(kup) ,fv(kxp)) 
tau=pv(loctauO+kph) 

fv(kMain)=tau*fv(kMain) ! time-scale only the rocket plant states 

if (kph . EQ . 3) then 

call ddpenMADS(nk,uv(locT) ,xv(locddpv) ,3,epsvdd,fv(locddpv) ,fv(locddpint)) 
endif 


where loctauO is set in the problem’s MODULE file as 
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qbarxIO qbarxIO 






Figure 11. Comparison of One- and Four-Phase g-constrained Cases with Jitter Penalty 


integer parameter : : & 

& loctau0=0 ! zero-base pointer for the taus for four phases 

2. Each phase may have a different pattern of equality and inequality constraints in 
cineq. Here, 

if (iecf lg. EQ . 1) then 
select case(kph) 
case (1) 

iecin=(/0, 1/) ! Tmax, q bounded 

case (2) 

iecin=(/l , 1 , 0/) ! T bounded, qmax 

case (3) 

iecin=(/l , 1 , 1/) ! T and q bounded (singular) 

case (4) 

iecin=(/0, 1/) ! Tmin, q bounded 

end select 
return 
endif 

call calcqbar(xj (kxp) ,qbar) 
select case(kph) 
case(l) 

cv(l)=Tmax-uv(locT) ! T=Tmax 

cv(2)=qbarmax-qbar ! qbarmax >= qbar 

case (2) 

cv(l)=Tmax-uv(locT) ! Tmax>=T 
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cv(2)=uv(locT) -Tmin 
cv(3)=qbarmax-qbar 
case (3) 

c v ( 1 ) =Tmax-uv ( lo cT) 
cv(2)=uv(locT)-Tmin 
cv(3)=qbarmax-qbar 
case (4) 

cv(l)=Tmin-uv(locT) 
cv(2)=qbarmax-qbar 
end select 

3. If the user is constructing the multi-phase problem from a single-phase one, it is critical 
not to forget that terminal boundary conditions no longer occur for xbc(kf (1)+ . . .). 
This is very easy to forget. The best policy is to express terminal values in terms of 
xbc(kf (nph)+ . . .). This is always correct, regardless of changes to the problem code. 
For phiobj, the code becomes 

rf =xbc (kf (nph)+locr) 

phi=-rf + Kddp*xbc(kf (3)+locddpint) ! ddpenMADS only on phase 3 

Note that because the ddpenMADS states only exist during phase three, the terminal 
value of the penalty integral is hard-wired to that phase. 

4. A multi-phase problem introduces boundary conditions at the intermediate phase 
boundaries. In this case, we require that the plant states be continuous, and we 
need to initialize the penalty integral xv(locddpint) , referred to in phiobj, above 
- to zero at the beginning of the third phase. The easiest way to handle continuity 
boundary conditions is to set up an index vector. Suppose that states 1, 3, and 4 are 
to be continuous across the phase 1/2 boundary. Define 

integer , parameter :: kl34(3)=(/l ,3,4/) 

and use it in psibc as follows: 

psi (kpsi+kl34)=xbc (kf (l)+kl34) -xbc (k0(2)+kl34) 
kpsi=kpsi+3 

where kpsi at the beginning of the code fragment was the number of psi elements 
defined thus far. For our problem, the relevant lines of psibc are 

if (iebcf lag .EQ . l)then 
iebc=0 

iebc(16: 19)=1 
return 
endif 

! initial conditions 
psi(l)=xbc(kO(l)+locr)-l 
psi(2)=xbc(kO(l)+locV) 
psi(3)=xbc(k0(l)+locm)-l 
kpsi=3 ! 3 

! continuity of plant traj from Tmax to qmax arcs 


! T>=Tmin 
! qbarmax=qbar 

! Tmax>=T 
! T>=Tmin 
! qbarmax>=qbar 

! T=Tmin 
! qbarmax>=qbar 
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psi (kpsi+kMain) =xbc (kf ( 1 ) +kMain) -xbc (kO (2) +kMain) 
kpsi=kpsi+nxp ! 6 

! continuity from qmax to singular 
psi (kpsi+kMain) =xbc (kf (2) +kMain) -xbc (kO (3) +kMain) 
kpsi=kpsi+nxp ! 9 

! initial condition for ddpenMADS integral 
psi (kpsi+l)=xbc (k0(3)+locddpint) 
kpsi=kpsi+l ! 10 

! continuity from singular to Tmin 

psi (kpsi+kMain) =xbc (kf (3) +kMain) -xbc (kO (4) +kMain) 
kpsi=kpsi+nxp ! 13 

! terminal conditions 
psi(kpsi+l)=xbc(kf (nph)+locV) 
psi (kpsi+2) =xbc (kf (nph) +locm) -mEmpty 
kpsi=kpsi+2 ! 15 

! bounds on taus 

do k=l,nph 

psi (kpsi+k)=pv(loctauO+k)-taumin 
enddo 

kpsi=kpsi+nph ! 19 

Note, in the fragment above, the initialization of the penalty integral as psi (10) and 
imposition of Tj > taumin, j = l,nph. The index vector kMain has already been 
defined for use in time-scaling the plant equations of motion. 

Although this multi-phase problem has a substantially more complicated temporal struc- 
ture than the corresponding single phase one, it is easy to create an initial guess, starting 
from a compatible single phase solution. Assume that we are starting from the solution for 
qbarmax = 9(10 -4 ), from Figure [7J in the upper right corner. The first step in preparing 
the initial guess for for the multi-phase problem is to plot whatever variable (frequently a 
control) most clearly displays the switching structure, simply against index, e.g., 

S=importMADS ( 1 SinglePhase_premads . dat 1 , ’ SinglePhase_vout . dat ’ , 0) ; 
plot (S. u{l}) ; grid on; 

The user then observes the index number(s) where the structure changes, and records them. 
For this problem, we have bv=[19 62 77] for four phases. The “S” structure and bv are 
saved together: 

save SinglePhase S bv 

and that . mat file is made available to a script like 

load SinglePhase 7. provide S and bv 
tau=S.p; 

bout=breaktrajMADS(S.x{l]-,S.u{l},bv,tau) ; °/ 0 Note that 

7, construct dimensions for four-phase initial guess 
P.nxv=[3 373]; / extra states for ddpenMADS in singular arc 

P.nuv=[l 111]; 

P.nav=[2 3 3 2]; "/, see cineq. . . 

P.kodev=[0 0 0 0]; 7, kodev only *needs* to be 0 during phase 3 
P.nbc=19; 

P . nph=4 ; 
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P . ndv=bout . ndv ; 

P.np=4; l p vector will hold four taus. 

randmag=0 ; 
vin= [] ; 

for k=l:P.nplL "/« Note adding extra states in phase 3 

vin= [vin ; adduxMADS (bout . x{k} , bout . u{k} , P . nxv (k) , P . nuv (k) , randmag) ] ; 
end 

vin= [vin ; bout . tau] ; 

save FourPhase_vin.dat vin -ascii -double 
name= ’ F ourPhase_pr emads . dat ’ ; 
writepremads (P .name) 

Subsection 2.3.2 describes how to use breaktrajMADS, andadduxMADS, and we have already 
used the latter in setting up the initial guess for the single-phase problem with ddpenMADS. 
The function breaktrajMADS is used to partition the trajectory into subintervals delimited 
at the the integration intervals given in bv. The output of breaktrajMADS,” bout,” includes 
the “ndv” vector, bout .ndv, and the vector of subinterval durations, bout .tau, in addition 
to the state and control trajectories for each subinterval. Note that the input guess has 
three states, so additional states are added only in the third phase, where P.nxv = 7. 

We now return to Figure 0 to compare the single-phase and four-phase solutions. 
Before that, though, a housekeeping comment is needed regarding the falloff in T in the 
four-phase case (upper right plot) at the end of the max —q arc. The falloff in T lasts for 
one integration interval, and induces a corresponding violation of the q = qbarmax equality 
constraint for that phase. Why did this happen? It is the result of computing q in cineq 
as a function of xj, the “current” value of the state when cineq is called. Recall that for a 
trajectory with nd integration intervals, the state appears at nd + 1 instants. By imposing 
the q constraint on xj values, the terminal value of q was unconstrained. This situation can 
be treated in several ways: 

• Brute Force 

The constraint could be doubled, imposed at both xj and at xjpl. This works, but 
is wasteful of computation, since it involves 2(nd — 1) redundant evaluations of the 
constraint. 

• Additional Boundary Condition 

The missing constraint could be imposed in psibc via 

call calcqbar (xbc (kf (2) +kxp) , qbar ) 

psi(kpsi+l)=qbarmax-qbar ! This element has iebcvec(kpsi+l)=0 

The is computationally efficient, but has a downside in that it requires that q be 
computed separately in psibc. 

• Compute the Constraint in xdot 

This issue does not come up when q is computed in xdot, because it results in the con- 
straint being imposed at the midpoints of the integration intervals: All state instants 
participate in the constraint. The downside, here, is that there is a vulnerability to 
control jitter on the active constraint arc. The jitter, as we’ve seen, can be treated 
using a penalty, as seen in this subsection. It is actually more effective, though, to 
constrain the constraint rate - in this case dq/dtt to zero. This will be demonstrated 
in the next subsection. 
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• Set the Problem Up Cleverly 

We observed the temporal pattern of active constraints in the single-phase case, before 
breaking it up into the four-phase problem, so we knew that the first phase, for which 
qbarmax > q , terminates with that constraint active, and that the state trajectory 
is continuous. Because of this, the first qbarmax = q constraint in the second phase 
is redundant, when q is computed using x j . We would do better in this case to use 
xjpl. The problem in Figure 0 can always be avoided by laying out the problem 
thoughtfully. 

Having described how we set up the four-phase problem, and how we should have set it 
up, we return to Figure [lT] to compare the single-phase and four-phase solutions. The most 
obvious differences appear in the thrust profiles in the Figure’s right column. The jitter- 
suppressed “singular” arc in the four-phase case is nothing like that in the single-phase one. 
Not only is it, to say the least, difficult to assign it a plausible physical interpretation, but 
it is very short. It follows an elongated max— q arc that carrys T all the way back to its 
T max boundary. This latter difference has a serious impact on the trajectory, increasing the 
time spent on the max— q constraint boundary by roughly 20%. This is visually evident in 
the Figure’s left column, in which the four-phase q history, at the top, essentially lacks the 
q dropoff during the singular arc that was seen in the bottom, single-phase, plot. 

Which of these trajectories is the (most) optimal one? It happens that the altitude gain 
for the single-phase problem is slightly less than .01% better than that for the four-phase 
one, but that’s certainly not very significant. If it is important to know what the optimal 
solution is, without the distortions of jitter penalties, misbehaving singular arcs, or other 
numerical artifacts, then it is necessary to formulate and solve the problem as a variational 
optimal control problem. While this can be quite difficult - indeed, practically impractical 
for practical problems - the next Subsection will use MADS COV support routines to lay 
out an approximate variational solution. 

Before leaving this Subsection, review what has been introduced here. This Subsection 
has focused on dealing with singular arcs using an heuristically motivated penalty function, 
implemented in ddpenMADS that targets large control accelerations accompanied by relatively 
small control rates. New material introduced in this Subsection included 

1. application of ddpenMADS to problems with an active state constraint arc implemented 
by computing the constraint function in xdot. It was seen that, while the measure 
nowhere produces perfect results, it benefits from gradually “walking the solution in;” 
that is, solving a sequence of problems with increasingly stringent constraint settings, 
using each solution as the initial guess for the next. 

2. The details of setting up a multiple-phase problem from a single-phase one were 
demonstrated, and various subtleties were discussed for posing state constraints in 
a multi-phase setting that uses cineq to compute the constraint function. 
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